continua implementazione homing

I parametri non vengono letti correttamente, leggere meglio
documentazione di rosparam

src/roboglue_ros/config/jointDefaults.yaml

@@ -8,9 +8,9 @@ jointNames:
     - wrist_3_joint
 #Home position of each joint in degrees
 jointHomePosition:
-    - shoulder_pan_joint: !!float 90.0
-    - shoulder_lift_joint: !!float -135.0
-    - elbow_joint: !!float 135.0
-    - wrist_1_joint: !!float -90.0
-    - wrist_2_joint: !!float -90.0
-    - wrist_3_joint: !!float 0.0
+     shoulder_pan_joint: !!float 90.0
+     shoulder_lift_joint: !!float -135.0
+     elbow_joint: !!float 135.0
+     wrist_1_joint: !!float -90.0
+     wrist_2_joint: !!float -90.0
+     wrist_3_joint: !!float 0.0

src/roboglue_ros/src/.gitignore

@@ -1 +0,0 @@
-*.cpp.autosave

src/roboglue_ros/src/roboglue_follower.cpp

@@ -51,6 +51,7 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
         } else if (!command.compare("HOME")) {
             // Chiamata alla funzione di reset dei giunti nel modello
             resetModelJointsHome(rdata);
+            rdata->kine_state->setVariablePositions(rdata->joint_homes);
             if (!params["execute"].get<bool>()){
                 ROS_DEBUG("Autosetting joints to Home Position");
                 ROS_WARN("Remote execution of homing not SAFE yet!!");
@@ -206,7 +207,7 @@ int main(int argc, char **argv) {
     nh.getParam(param_ns+"OUTtraj_jog", ros_topics.traj_jog);
     nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
     nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
-    nh.getParam(param_ns+"joint_defaults/jointHomePosition", robot_data.joint_homes);
+    nh.getParam(param_ns+"jointHomePosition", robot_data.joint_homes);
     nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate);
 
     /// set spinner rate ///
@@ -264,11 +265,9 @@ int main(int argc, char **argv) {
 
 void resetModelJointsHome(robotData* rdata) {
     ROS_DEBUG("Setting joint position to home");
-    std::map<std::string, double> jhome = rdata->joint_homes;
-    for (auto joint : jhome) {
-        std::string jname = joint.first;
-        const double jval = deg2rad(joint.second);
-        rdata->kine_state->setJointPositions(jname, &jval);
+    for(auto joint : rdata->joint_homes){
+        joint.second = deg2rad(joint.second);
+        rdata->joint_homes[joint.first] = joint.second;
     }
 }