add joint_ctrl_mode

Author: alaurenzi

launch/cartesio.launch.xml

@@ -12,16 +12,15 @@
     <arg name="solution_topic" default="!"/>
     <arg name="enable_otg" default="true"/>
     <arg name="problem_description_filename"/>
-    <arg name="home" default="$(var home)"/>
-
-
+    <arg name="home" default="home"/>
+    <arg name="require_srdf" default="true"/>
+    <arg name="visual_mode" default="false"/>
+    <arg name="joint_ctrl_mode" default="{}"/>
 
     <node pkg="cartesian_interface_ros" exec="ros_server_node"
                                     name="ros_server_node"
                                     output="screen"
                                     launch-prefix="$(var prefix)">
-<!--        <remap from="robot_description" to="/xbotcore/robot_description"/>-->
-<!--        <remap from="robot_description_semantic" to="/xbotcore/robot_description_semantic"/>-->
         <param name="enable_otg" value="$(var enable_otg)"/>
         <param name="is_model_floating_base" value="$(var is_model_floating_base)"/>
         <param name="model_type" value="pin"/>
@@ -31,16 +30,13 @@
         <param name="tf_prefix" value="$(var tf_prefix)"/>
         <param name="problem_description" type="str" value="$(command 'cat $(var problem_description_filename)')"/>
         <param name="home" value="$(var home)" type="str"/>
-<!--            <param if="$(eval arg('world_frame_link') != '!')" name="world_frame_link" value="$(arg world_frame_link)"/>-->
-<!--            <remap if="$(eval arg('solution_topic') != '!')" from="cartesian/solution" to="$(arg solution_topic)"/>-->
-
+        <param name="require_srdf" value="$(var require_srdf)"/>
+        <param name="visual_mode" value="$(var visual_mode)"/>
+        <param name="joint_ctrl_mode" type="str" value="$(var joint_ctrl_mode)"/>
     </node>
 
     <node if="$(var markers)" pkg="cartesian_interface_ros" exec="marker_spawner" name="interactive_markers" output="screen">
         <param name="tf_prefix" value="$(var tf_prefix)"/>
     </node>
 
-
-
-
 </launch>

src/ros/RosExecutor.cpp

@@ -172,8 +172,24 @@ void RosExecutor::init_customize_command()
         {
             ctrl_map[j] = XBot::ControlMode::Position() + XBot::ControlMode::Velocity();
         }
+    }
+
+    std::string joint_ctrl_mode_str = "{}";
+    _node->get_parameter("joint_ctrl_mode", joint_ctrl_mode_str);
+    auto joint_ctrl_mode_map = YAML::Load(joint_ctrl_mode_str).as<std::map<std::string, int>>();
+
+    for(auto pair : joint_ctrl_mode_map)
+    {
+        auto j = pair.first;
+        auto mode_int = pair.second;
 
+        if(!_robot->hasJoint(j))
+        {
+            throw std::runtime_error("Joint ctrl mode map contains non existing joint '" + j + "'");
+        }
 
+        XBot::ControlMode::Type mode = static_cast<XBot::ControlMode::Type>(mode_int);
+        ctrl_map[j] = mode;
     }
 
     _robot->setControlMode(ctrl_map);