Add gripper joint state publisher to franka_visualization

CHANGELOG.md

@@ -8,6 +8,7 @@ Requires `libfranka` >= 0.3.0
   * Added missing dependency to `panda_moveit_config`
   * Fixed linker errors when building with `-DFranka_DIR` while an older version of
     `ros-kinetic-libfranka` is installed
+  * Added gripper joint state publisher to `franka_visualization`
 
 ## 0.2.2 - 2018-01-31
 

franka_visualization/CMakeLists.txt

@@ -13,24 +13,28 @@ find_package(Franka 0.3.0 REQUIRED)
 
 catkin_package(CATKIN_DEPENDS sensor_msgs roscpp)
 
-add_executable(franka_joint_state_publisher
-  src/franka_joint_state_publisher.cpp
+set(EXECUTABLES robot_joint_state_publisher gripper_joint_state_publisher)
+
+foreach(executable ${EXECUTABLES})
+  add_executable(${executable}
+    src/${executable}.cpp
   )
-add_dependencies(franka_joint_state_publisher
+  add_dependencies(${executable}
     ${${PROJECT_NAME}_EXPORTED_TARGETS}
     ${catkin_EXPORTED_TARGETS}
   )
-target_include_directories(franka_joint_state_publisher SYSTEM PUBLIC
+  target_include_directories(${executable} SYSTEM PUBLIC
     ${Franka_INCLUDE_DIRS}
     ${catkin_INCLUDE_DIRS}
   )
-target_link_libraries(franka_joint_state_publisher PUBLIC
+  target_link_libraries(${executable} PUBLIC
     ${Franka_LIBRARIES}
     ${catkin_LIBRARIES}
   )
+endforeach()
 
 ## Installation
-install(TARGETS franka_joint_state_publisher
+install(TARGETS ${EXECUTABLES}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -55,6 +59,6 @@ if(CLANG_TOOLS)
   add_format_target(franka_visualization FILES ${SOURCES} ${HEADERS})
   add_tidy_target(franka_visualization
     FILES ${SOURCES}
-    DEPENDS franka_joint_state_publisher
+    DEPENDS ${EXECUTABLES}
   )
 endif()

franka_visualization/config/gripper_settings.yaml

+joint_names:
+  - panda_finger_joint1
+  - panda_finger_joint2

franka_visualization/launch/franka_visualization.launch

@@ -3,20 +3,28 @@
 <launch>
   <arg name="load_gripper" default="true" />
   <arg name="robot_ip" default="robot.franka.de" />
+  <arg name="publish_rate" default="30" />
 
   <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" />
   <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
 
-  <node name="franka_joint_state_publisher" pkg="franka_visualization" type="franka_joint_state_publisher" output="screen">
-    <rosparam command="load" file="$(find franka_visualization)/config/robot_read_settings.yaml" />
+  <node name="robot_joint_state_publisher" pkg="franka_visualization" type="robot_joint_state_publisher" output="screen">
+    <rosparam command="load" file="$(find franka_visualization)/config/robot_settings.yaml" />
     <param name="robot_ip" value="$(arg robot_ip)" />
+    <param name="publish_rate" value="$(arg publish_rate)" />
+  </node>
+
+  <node name="gripper_joint_state_publisher" pkg="franka_visualization" type="gripper_joint_state_publisher" output="screen" if="$(arg load_gripper)">
+    <rosparam command="load" file="$(find franka_visualization)/config/gripper_settings.yaml" />
+    <param name="robot_ip" value="$(arg robot_ip)" />
+    <param name="publish_rate" value="$(arg publish_rate)" />
   </node>
 
   <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
     <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />
     <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" />
-    <rosparam  param="source_list">[franka_joint_state_publisher/joint_states] </rosparam>
-    <param  if="$(arg load_gripper)" name="publish_default_positions" value="true" />
+    <param name="rate" value="$(arg publish_rate)" />
+    <rosparam param="source_list">[franka_joint_state_publisher/joint_states, franka_gripper_state_publisher/joint_states]</rosparam>
   </node>
 
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

franka_visualization/src/gripper_joint_state_publisher.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka/exception.h>
+#include <franka/gripper.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "gripper_joint_state_publisher");
+  ros::NodeHandle node_handle("~");
+
+  std::vector<std::string> joint_names;
+  node_handle.getParam("joint_names", joint_names);
+
+  std::string robot_ip;
+  node_handle.getParam("robot_ip", robot_ip);
+
+  double publish_rate;
+  node_handle.getParam("publish_rate", publish_rate);
+
+  ros::Rate rate(publish_rate);
+
+  sensor_msgs::JointState states;
+  states.effort.resize(joint_names.size());
+  states.name.resize(joint_names.size());
+  states.position.resize(joint_names.size());
+  states.velocity.resize(joint_names.size());
+
+  for (size_t i = 0; i < joint_names.size(); i++) {
+    states.name[i] = joint_names[i];
+  }
+
+  ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+  try {
+    franka::Gripper gripper(robot_ip);
+
+    while (ros::ok()) {
+      franka::GripperState gripper_state = gripper.readOnce();
+      states.header.stamp = ros::Time::now();
+      for (size_t i = 0; i < joint_names.size(); i++) {
+        states.position[i] = gripper_state.width * 0.5;
+        states.velocity[i] = 0.0;
+        states.effort[i] = 0.0;
+      }
+      publisher.publish(states);
+      ros::spinOnce();
+      rate.sleep();
+    }
+
+  } catch (const franka::Exception& e) {
+    ROS_ERROR_STREAM("Exception: " << e.what());
+    return -1;
+  }
+
+  return 0;
+}

franka_visualization/src/franka_joint_state_publisher.cpp → franka_visualization/src/robot_joint_state_publisher.cpp

@@ -6,54 +6,50 @@
 #include <sensor_msgs/JointState.h>
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "franka_joint_state_publisher");
-  ros::NodeHandle private_nodehandle("~");
-  ros::Rate rate(30.0);
-  ros::Publisher joint_pub =
-
-      private_nodehandle.advertise<sensor_msgs::JointState>("joint_states", 1);
-  const size_t kNumberOfJoints = 7;
-  std::array<std::string, kNumberOfJoints> joint_names;
-  XmlRpc::XmlRpcValue params;
-  private_nodehandle.getParam("joint_names", params);
-
-  for (size_t i = 0; i < kNumberOfJoints; i++) {
-    joint_names[i] = static_cast<std::string>(params[i]);
-  }
+  ros::init(argc, argv, "robot_joint_state_publisher");
+  ros::NodeHandle node_handle("~");
+
+  std::vector<std::string> joint_names;
+  node_handle.getParam("joint_names", joint_names);
 
   std::string robot_ip;
-  private_nodehandle.getParam("robot_ip", robot_ip);
+  node_handle.getParam("robot_ip", robot_ip);
+
+  double publish_rate;
+  node_handle.getParam("publish_rate", publish_rate);
+
+  ros::Rate rate(publish_rate);
 
   sensor_msgs::JointState states;
-  states.effort.resize(kNumberOfJoints);
-  states.name.resize(kNumberOfJoints);
-  states.position.resize(kNumberOfJoints);
-  states.velocity.resize(kNumberOfJoints);
+  states.effort.resize(joint_names.size());
+  states.name.resize(joint_names.size());
+  states.position.resize(joint_names.size());
+  states.velocity.resize(joint_names.size());
 
   for (size_t i = 0; i < joint_names.size(); i++) {
     states.name[i] = joint_names[i];
   }
 
+  ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
+
   try {
     franka::Robot robot(robot_ip);
 
-    uint32_t sequence_number = 0;
     robot.read([&](const franka::RobotState& robot_state) {
       states.header.stamp = ros::Time::now();
-      states.header.seq = sequence_number++;
-      for (size_t i = 0; i < kNumberOfJoints; i++) {
+      for (size_t i = 0; i < joint_names.size(); i++) {
         states.position[i] = robot_state.q[i];
         states.velocity[i] = robot_state.dq[i];
         states.effort[i] = robot_state.tau_J[i];
       }
-      joint_pub.publish(states);
+      publisher.publish(states);
       ros::spinOnce();
       rate.sleep();
       return ros::ok();
     });
 
   } catch (const franka::Exception& e) {
-    ROS_ERROR_STREAM("" << e.what());
+    ROS_ERROR_STREAM("Exception: " << e.what());
     return -1;
   }