Add start_position_parameter

- Set start position of Nextage Open

fake_joint_driver/README.md

@@ -48,7 +48,7 @@ sliders to set the joint trajectory goals.
 
 # Parameters
 
-- use_robot_description
+- `use_robot_description`
 
   When it is true, the driver read the parameter `/robot_description`
   to obtain the joint list to control by this driver. When it is
@@ -64,6 +64,21 @@ sliders to set the joint trajectory goals.
   This is the list of the joints which the driver node should not
   control.
 
+- `start_position`
+
+  This is a map to specify the initial position of the joint. You can
+  use this to avoid collision in zero joint angle.
+
+  Use like:
+
+  ```
+  <!-- fake_joint_driver_node -->
+  <node name="fake_joint_driver" pkg="fake_joint_driver" type="fake_joint_driver_node">
+    <!-- Set joint start position -->
+    <rosparam param="start_position">{LARM_JOINT2: -1.7, RARM_JOINT2: -1.7}</rosparam>
+  </node>
+  ```
+
 # Caution
 
 You can set target joints for joint_trajectory_controller in the

fake_joint_driver/src/fake_joint_driver.cpp

@@ -17,11 +17,18 @@ FakeJointDriver::FakeJointDriver(void)
 {
   ros::NodeHandle pnh("~");
   std::set<std::string> joint_set;
+  std::map<std::string, double> start_position_map;
 
   // Read parameters
   pnh.param<bool>("use_robot_description", use_description_, true);
   pnh.getParam("include_joints", include_joints_);
   pnh.getParam("exclude_joints", exclude_joints_);
+  pnh.getParam("start_position", start_position_map);
+
+  for (auto it=start_position_map.begin(); it!=start_position_map.end(); it++)
+  {
+    ROS_DEBUG_STREAM("start_position: " << it->first << ": " << it->second);
+  }
 
   for (auto i=0; i<include_joints_.size(); i++)
   {
@@ -76,10 +83,23 @@ FakeJointDriver::FakeJointDriver(void)
   act_vel.resize(joint_names_.size());
   act_eff.resize(joint_names_.size());
 
+  // Set start position
+  for (auto it=start_position_map.begin(); it!=start_position_map.end(); it++)
+  {
+    for (auto i=0; i<joint_names_.size(); i++)
+    {
+      if (joint_names_[i] == it->first)
+      {
+        act_dis[i] = it->second;
+        cmd_dis[i] = it->second;
+      }
+    }
+  }
+
   // Create joint_state_interface and position_joint_interface
   for (int i = 0; i< joint_names_.size(); i++)
   {
-    ROS_INFO_STREAM("joint[" << i << "]:" << joint_names_[i]);
+    ROS_DEBUG_STREAM("joint[" << i << "]:" << joint_names_[i]);
     // Connect and register the joint_state_interface
     hardware_interface::JointStateHandle state_handle(joint_names_[i], &act_dis[i], &act_vel[i], &act_eff[i]);
     joint_state_interface.registerHandle(state_handle);

fake_joint_launch/launch/nextage.launch

@@ -16,6 +16,8 @@
 
   <!-- fake_joint_driver_node -->
   <node name="fake_joint_driver" pkg="fake_joint_driver" type="fake_joint_driver_node">
+    <!-- Set joint start position -->
+    <rosparam param="start_position">{LARM_JOINT2: -1.7, RARM_JOINT2: -1.7}</rosparam>
   </node>
   <!-- robot_state_publisher -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>