Added tau_J_d, theta and dtheta to the FrankaState message.

CHANGELOG.md

@@ -4,8 +4,8 @@
 
   * Added missing run-time dependencies to `franka_description` and `franka_control`
   * Added epsilon to grasp action
-  * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
-    to the robot state
+  * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`,
+    `theta` and `dtheta` to `franka::RobotState`
   * Updated and improved examples
   * Fixed includes for Eigen3 in `franka_example_controllers`
   * Fixed gripper state publishing in `franka_gripper_node`

franka_control/src/franka_state_controller.cpp

@@ -255,6 +255,12 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
                   "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
+                  "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
@@ -268,6 +274,9 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
       publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
       publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
       publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
+      publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
+      publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
+      publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
       publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
       publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
       publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];

franka_msgs/msg/FrankaState.msg

@@ -5,8 +5,11 @@ float64[7] q
 float64[7] q_d
 float64[7] dq
 float64[7] dq_d
+float64[7] theta
+float64[7] dtheta
 float64[7] tau_J
 float64[7] dtau_J
+float64[7] tau_J_d
 float64[6] K_F_ext_hat_K
 float64[2] elbow
 float64[2] elbow_d