Remove arm_id from launchfiles, change franka_control namespacing

CHANGELOG.md

@@ -2,6 +2,8 @@
 
 ## 0.4.0 - UNRELEASED
 
+  * **BREAKING** Removed `arm_id` from launchfiles
+  * **BREAKING** Changed namespace of `franka_control` controller manager
   * Changes in `panda_moveit_config`:
     * Updated joint limits from URDF
     * Removed `home` poses

franka_control/config/default_controllers.yaml

@@ -63,4 +63,12 @@ effort_joint_trajectory_controller:
 franka_state_controller:
   type: franka_control/FrankaStateController
   publish_rate: 30  # [Hz]
-
+  joint_names:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+  arm_id: panda

franka_control/launch/franka_control.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" default="panda" />
   <arg name="load_gripper" default="true" />
 
-  <node name="$(arg arm_id)" pkg="franka_control" type="franka_control_node" output="screen" required="true">
-    <rosparam command="load" file="$(find franka_control)/config/franka_control.yaml" />
-    <param name="robot_ip" value="$(arg robot_ip)" />
-    <param name="arm_id" value="$(arg arm_id)" />
   <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)" />
+
+  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
+    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
+    <param name="robot_ip" value="$(arg robot_ip)" />
   </node>
 
-  <group ns="$(arg arm_id)">
   <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
   <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">
-      <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />
-      <rosparam  if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
+    <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>
     <param name="rate" value="30"/>
   </node>
-  </group>
 </launch>

franka_control/src/franka_control_node.cpp

@@ -30,7 +30,8 @@ class ServiceContainer {
 };
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "franka_hw");
+  ros::init(argc, argv, "franka_control_node");
+  ros::NodeHandle public_node_handle;
   ros::NodeHandle node_handle("~");
 
   std::vector<std::string> joint_names_vector;
@@ -42,9 +43,16 @@ int main(int argc, char** argv) {
   std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
 
   std::string robot_ip;
-  node_handle.getParam("robot_ip", robot_ip);
+  if (!node_handle.getParam("robot_ip", robot_ip)) {
+    ROS_ERROR("Invalid or no robot_ip parameter provided");
+    return 1;
+  }
+
   std::string arm_id;
-  node_handle.getParam("arm_id", arm_id);
+  if (!node_handle.getParam("arm_id", arm_id)) {
+    ROS_ERROR("Invalid or no arm_id parameter provided");
+    return 1;
+  }
   franka::Robot robot(robot_ip);
 
   // Set default collision behavior
@@ -87,6 +95,7 @@ int main(int argc, char** argv) {
           robot.automaticErrorRecovery();
           has_error = false;
           recovery_action_server.setSucceeded();
+          ROS_INFO("Recovered from error");
         } catch (const franka::Exception& ex) {
           recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
         }
@@ -94,12 +103,12 @@ int main(int argc, char** argv) {
       false);
 
   franka::Model model = robot.loadModel();
-  franka_hw::FrankaHW franka_control(joint_names, arm_id, node_handle, model);
+  franka_hw::FrankaHW franka_control(joint_names, arm_id, public_node_handle, model);
 
   // Initialize robot state before loading any controller
   franka_control.update(robot.readOnce());
 
-  controller_manager::ControllerManager control_manager(&franka_control, node_handle);
+  controller_manager::ControllerManager control_manager(&franka_control, public_node_handle);
 
   recovery_action_server.start();
 

franka_control/src/franka_state_controller.cpp

@@ -151,7 +151,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
     ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
     return false;
   }
-  if (!root_node_handle.getParam("arm_id", arm_id_)) {
+  if (!controller_node_handle.getParam("arm_id", arm_id_)) {
     ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
     return false;
   }
@@ -163,7 +163,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
                     << publish_rate << " [Hz].");
   }
 
-  if (!root_node_handle.getParam("joint_names", joint_names_) ||
+  if (!controller_node_handle.getParam("joint_names", joint_names_) ||
       joint_names_.size() != robot_state_.q.size()) {
     ROS_ERROR(
         "FrankaStateController: Invalid or no joint_names parameters provided, aborting "

franka_example_controllers/launch/move_to_start.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" default="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>
   <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
-    <arg name="arm_id" value="$(arg arm_id)" />
   </include>
-  <group ns="$(arg arm_id)">
   <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
-  </group>
 </launch>

franka_gripper/launch/franka_gripper.launch

 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" default="robot.franka.de" />
-  <arg name="arm_id" default="panda" />
-  <group ns="$(arg arm_id)">
-    <node name="franka_gripper_node" pkg="franka_gripper" type="franka_gripper_node" output="screen">
+
+  <node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen">
     <param name="robot_ip" value="$(arg robot_ip)"/>
-      <rosparam command="load" file="$(find franka_gripper)/config/gripper_node_config.yaml" />
+    <rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" />
   </node>
-  </group>
 </launch>

franka_visualization/launch/franka_visualization.launch

@@ -21,8 +21,6 @@
   </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'" />
     <param name="rate" value="$(arg publish_rate)" />
     <rosparam param="source_list">[robot_joint_state_publisher/joint_states, gripper_joint_state_publisher/joint_states]</rosparam>
   </node>

panda_moveit_config/launch/moveit_rviz.launch

@@ -8,13 +8,9 @@
   <arg unless="$(arg config)" name="command_args" value="" />
   <arg     if="$(arg config)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit.rviz" />
 
-  <arg name="arm_id" default="panda" />
-
-  <group ns="$(arg arm_id)">
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
       args="$(arg command_args)" output="screen">
     <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
   </node>
-  </group>
 
 </launch>

panda_moveit_config/launch/panda_moveit.launch

 <?xml version="1.0" ?>
 <launch>
-  <arg name="arm_id" default="panda" />
   <!-- Valid values: "position", "effort" -->
   <arg name="controller" default="position" />
 
-  <group ns="$(arg arm_id)">
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)_joint_trajectory_controller"/>
   <include file="$(find panda_moveit_config)/launch/move_group.launch" />
-  </group>
 </launch>