Add joint_state_publisher for desired values

franka_control/include/franka_control/franka_state_controller.h

@@ -61,6 +61,7 @@ class FrankaStateController
   realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
   realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_;
   realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
+  realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_desired_;
   realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> publisher_external_wrench_;
   franka_hw::TriggerRate trigger_publish_;
   franka::RobotState robot_state_;

franka_control/launch/franka_control.launch

@@ -2,6 +2,7 @@
 <launch>
   <arg name="robot_ip" />
   <arg name="load_gripper" default="true" />
+  <arg name="publish_desired_values" default="false" />
 
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
@@ -15,8 +16,10 @@
   <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
   <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
-    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
-    <rosparam  unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
+    <rosparam if="$(eval load_gripper and publish_desired_values)" param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
+    <rosparam if="$(eval load_gripper and not publish_desired_values)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
+    <rosparam if="$(eval not load_gripper and publish_desired_values)" param="source_list">[franka_state_controller/joint_states_desired] </rosparam>
+    <rosparam if="$(eval not load_gripper and not publish_desired_values)" param="source_list">[franka_state_controller/joint_states] </rosparam>
     <param name="rate" value="30"/>
   </node>
 </launch>

franka_control/src/franka_state_controller.cpp

@@ -140,6 +140,7 @@ FrankaStateController::FrankaStateController()
       publisher_transforms_(),
       publisher_franka_states_(),
       publisher_joint_states_(),
+      publisher_joint_states_desired_(),
       publisher_external_wrench_(),
       trigger_publish_(30.0) {}
 
@@ -182,6 +183,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
   publisher_transforms_.init(root_node_handle, "/tf", 1);
   publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
   publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
+  publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1);
   publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
 
   {
@@ -192,6 +194,14 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
     publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
     publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
   }
+  {
+    std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
+        publisher_joint_states_desired_);
+    publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());
+    publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());
+    publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());
+    publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());
+  }
   {
     std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> > lock(
         publisher_transforms_);
@@ -352,6 +362,21 @@ void FrankaStateController::publishJointStates(const ros::Time& time) {
     publisher_joint_states_.msg_.header.seq = sequence_number_;
     publisher_joint_states_.unlockAndPublish();
   }
+  if (publisher_joint_states_desired_.trylock()) {
+    static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d),
+                  "Robot state joint members do not have same size");
+    for (size_t i = 0; i < robot_state_.q_d.size(); i++) {
+      publisher_joint_states_desired_.msg_.name[i] = joint_names_[i];
+      publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i];
+      publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i];
+      publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i];
+    }
+    publisher_joint_states_desired_.msg_.header.stamp = time;
+    publisher_joint_states_desired_.msg_.header.seq = sequence_number_;
+    publisher_joint_states_desired_.unlockAndPublish();
+  }
 }
 
 void FrankaStateController::publishTransforms(const ros::Time& time) {

franka_example_controllers/launch/move_to_start.launch

@@ -5,6 +5,7 @@
   <include file="$(find franka_control)/launch/franka_control.launch">
     <arg name="robot_ip" value="$(arg robot_ip)" />
     <arg name="load_gripper" value="false" />
+    <arg name="publish_desired_values" value="true" />
   </include>
   <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
     <arg name="load_gripper" value="false" />