diff --git a/CHANGELOG.md b/CHANGELOG.md
index 12045cb7ff558e22d41a1049f621ed80bdd66927..9109c475d6411e3a8135f2ca9c2e4375e9b01164 100644
--- a/CHANGELOG.md
+++ b/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
diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml
index e3a3f947df83346b42516b20258874c1a1a6e33e..533a31c51359428a4a14f797bd2db5e52640befd 100644
--- a/franka_control/config/default_controllers.yaml
+++ b/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
diff --git a/franka_control/config/franka_control.yaml b/franka_control/config/franka_control_node.yaml
similarity index 100%
rename from franka_control/config/franka_control.yaml
rename to franka_control/config/franka_control_node.yaml
diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
index 1b2fe028515f3a31e33037665c0e09f0020c99ae..dbc6d192af7060650caf43b86b4ea162874d6af6 100644
--- a/franka_control/launch/franka_control.launch
+++ b/franka_control/launch/franka_control.launch
@@ -1,26 +1,22 @@
<?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_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)" />
- <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>
- <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 unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
- <param name="rate" value="30"/>
- </node>
- </group>
+ <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">
+ <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>
</launch>
diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp
index ac442663b4df70f9eaca462c97497494dce961ae..619268609fccf426869b47a859799e8d18e84a5b 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/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();
diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp
index 2faa5aea54f6cf4c66a145cc0c2b844de04f4ef1..f7af454c738370112b3e2c8adea005f62b1666ab 100644
--- a/franka_control/src/franka_state_controller.cpp
+++ b/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 "
diff --git a/franka_example_controllers/launch/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch
index ed8accb2b7fd8bc9d4e7636b08fc210e5c2f7601..15c8f66dab4e7a618b3f9746e5227dbd5f9e8a25 100644
--- a/franka_example_controllers/launch/move_to_start.launch
+++ b/franka_example_controllers/launch/move_to_start.launch
@@ -1,18 +1,13 @@
<?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>
+ <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
</launch>
diff --git a/franka_gripper/config/gripper_node_config.yaml b/franka_gripper/config/franka_gripper_node.yaml
similarity index 100%
rename from franka_gripper/config/gripper_node_config.yaml
rename to franka_gripper/config/franka_gripper_node.yaml
diff --git a/franka_gripper/launch/franka_gripper.launch b/franka_gripper/launch/franka_gripper.launch
index d990574d4c8511c770b9612fe60a074a25fbb1ad..93c9e43474a0e62e209b28b0c1bf24e2e8182552 100644
--- a/franka_gripper/launch/franka_gripper.launch
+++ b/franka_gripper/launch/franka_gripper.launch
@@ -1,11 +1,9 @@
<?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">
- <param name="robot_ip" value="$(arg robot_ip)"/>
- <rosparam command="load" file="$(find franka_gripper)/config/gripper_node_config.yaml" />
- </node>
- </group>
+
+ <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/franka_gripper_node.yaml" />
+ </node>
</launch>
diff --git a/franka_visualization/launch/franka_visualization.launch b/franka_visualization/launch/franka_visualization.launch
index a713f4fd5963bbe88cad4a795130220b08ab10d0..4bbee5d6703fb830bd89efbbcaf053cc486134dc 100644
--- a/franka_visualization/launch/franka_visualization.launch
+++ b/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>
diff --git a/panda_moveit_config/launch/moveit_rviz.launch b/panda_moveit_config/launch/moveit_rviz.launch
index 71f196efab7eb3e525978ab35d8d1f31a0d4e018..17387f9e2923ca48cf2cb8feab98e92c0e297491 100644
--- a/panda_moveit_config/launch/moveit_rviz.launch
+++ b/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>
+ <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>
</launch>
diff --git a/panda_moveit_config/launch/panda_moveit.launch b/panda_moveit_config/launch/panda_moveit.launch
index a879467f3b7a9e5122e095ccf36f14231585e8da..cab708e88cb73ec9666391a4eef243cacd64c762 100644
--- a/panda_moveit_config/launch/panda_moveit.launch
+++ b/panda_moveit_config/launch/panda_moveit.launch
@@ -1,11 +1,8 @@
<?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>
+ <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" />
</launch>