Update example controllers

franka_example_controllers/config/franka_example_controllers.yaml

 joint_velocity_example_controller:
     type: franka_example_controllers/JointVelocityExampleController
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
 
 joint_position_example_controller:
     type: franka_example_controllers/JointPositionExampleController
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
 
 cartesian_pose_example_controller:
     type: franka_example_controllers/CartesianPoseExampleController
+    arm_id: panda
 
 cartesian_velocity_example_controller:
     type: franka_example_controllers/CartesianVelocityExampleController
+    arm_id: panda
 
 model_example_controller:
     type: franka_example_controllers/ModelExampleController

franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h

@@ -18,8 +18,7 @@ class JointPositionExampleController : public controller_interface::MultiInterfa
                                            hardware_interface::PositionJointInterface> {
  public:
   bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+            ros::NodeHandle& node_handle) override;
   void starting(const ros::Time&) override;
   void update(const ros::Time&, const ros::Duration& period) override;
 

franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h

@@ -19,8 +19,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
                                            franka_hw::FrankaStateInterface> {
  public:
   bool init(hardware_interface::RobotHW* robot_hardware,
-            ros::NodeHandle& root_node_handle,
-            ros::NodeHandle& /* controller_node_handle */) override;
+            ros::NodeHandle& node_handle) override;
   void update(const ros::Time&, const ros::Duration& period) override;
   void starting(const ros::Time&) override;
   void stopping(const ros::Time&) override;

franka_example_controllers/launch/cartesian_impedance_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
-    <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen" />
+  <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
+    <param name="link_name" value="panda_link0" />
+  </node>
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-  </group>
 </launch>

franka_example_controllers/launch/cartesian_pose_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="cartesian_pose_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/cartesian_velocity_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="cartesian_velocity_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/force_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="force_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
-  </group>
 </launch>

franka_example_controllers/launch/joint_impedance_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_impedance_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/joint_position_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_position_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/joint_velocity_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_velocity_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/model_example_controller.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" value="panda" />
   <arg name="load_gripper" default="true" />
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
-    <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
-  <group ns="$(arg arm_id)">
+
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="model_example_controller"/>
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
-  </group>
 </launch>

franka_example_controllers/launch/rviz/franka_description_with_marker.rviz

@@ -115,7 +115,7 @@ Visualization Manager:
       Show Axes: false
       Show Descriptions: true
       Show Visual Aids: false
-      Update Topic: /panda/equilibrium_pose_marker/update
+      Update Topic: /equilibrium_pose_marker/update
       Value: true
   Enabled: true
   Global Options:

franka_example_controllers/scripts/interactive_marker.py

@@ -18,8 +18,8 @@ pose_pub = None
 position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
 
 
-def publisherCallback(msg, arm_id):
-    marker_pose.header.frame_id = arm_id + "_link0"
+def publisherCallback(msg, link_name):
+    marker_pose.header.frame_id = link_name
     marker_pose.header.stamp = rospy.Time(0)
     pose_pub.publish(marker_pose)
 
@@ -61,7 +61,7 @@ if __name__ == "__main__":
     state_sub = rospy.Subscriber("franka_state_controller/franka_states",
                                  FrankaState, franka_state_callback)
     listener = tf.TransformListener()
-    arm_id = rospy.get_param("arm_id")
+    link_name = rospy.get_param("~link_name")
 
     # Get initial pose for the interactive marker
     while not initial_pose_found:
@@ -69,10 +69,10 @@ if __name__ == "__main__":
     state_sub.unregister()
 
     pose_pub = rospy.Publisher(
-        "/equilibrium_pose", PoseStamped, queue_size=10)
+        "equilibrium_pose", PoseStamped, queue_size=10)
     server = InteractiveMarkerServer("equilibrium_pose_marker")
     int_marker = InteractiveMarker()
-    int_marker.header.frame_id = arm_id + "_link0"
+    int_marker.header.frame_id = link_name
     int_marker.scale = 0.3
     int_marker.name = "equilibrium_pose"
     int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
@@ -82,7 +82,7 @@ if __name__ == "__main__":
     int_marker.pose = marker_pose.pose
     # run pose publisher
     rospy.Timer(rospy.Duration(0.005),
-                lambda msg: publisherCallback(msg, arm_id))
+                lambda msg: publisherCallback(msg, link_name))
 
     # insert a box
     control = InteractiveMarkerControl()

franka_example_controllers/src/joint_position_example_controller.cpp

@@ -13,8 +13,7 @@
 namespace franka_example_controllers {
 
 bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
-                                          ros::NodeHandle& root_node_handle,
-                                          ros::NodeHandle& /* controller_node_handle */) {
+                                          ros::NodeHandle& node_handle) {
   position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
   if (position_joint_interface_ == nullptr) {
     ROS_ERROR(
@@ -22,7 +21,7 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
     return false;
   }
   std::vector<std::string> joint_names;
-  if (!root_node_handle.getParam("joint_names", joint_names)) {
+  if (!node_handle.getParam("joint_names", joint_names)) {
     ROS_ERROR("JointPositionExampleController: Could not parse joint names");
   }
   if (joint_names.size() != 7) {

franka_example_controllers/src/joint_velocity_example_controller.cpp

@@ -13,8 +13,7 @@
 namespace franka_example_controllers {
 
 bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
-                                          ros::NodeHandle& root_node_handle,
-                                          ros::NodeHandle& /* controller_node_handle */) {
+                                          ros::NodeHandle& node_handle) {
   velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
   if (velocity_joint_interface_ == nullptr) {
     ROS_ERROR(
@@ -22,7 +21,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
     return false;
   }
   std::vector<std::string> joint_names;
-  if (!root_node_handle.getParam("joint_names", joint_names)) {
+  if (!node_handle.getParam("joint_names", joint_names)) {
     ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
   }
   if (joint_names.size() != 7) {
@@ -41,12 +40,6 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
     }
   }
 
-  std::string arm_id;
-  if (!root_node_handle.getParam("arm_id", arm_id)) {
-    ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id");
-    return false;
-  }
-
   auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
   if (state_interface == nullptr) {
     ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
@@ -54,7 +47,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
   }
 
   try {
-    auto state_handle = state_interface->getHandle(arm_id + "_robot");
+    auto state_handle = state_interface->getHandle("panda_robot");
 
     std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
     for (size_t i = 0; i < q_start.size(); i++) {