From 3f695940b95af8b72fcefc2f65ec6f31da5a1b80 Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Thu, 8 Mar 2018 16:17:21 +0100
Subject: [PATCH] Remove arm_id from launchfiles, change franka_control
namespacing
---
CHANGELOG.md | 2 ++
.../config/default_controllers.yaml | 10 ++++++-
..._control.yaml => franka_control_node.yaml} | 0
franka_control/launch/franka_control.launch | 30 ++++++++-----------
franka_control/src/franka_control_node.cpp | 19 ++++++++----
.../src/franka_state_controller.cpp | 4 +--
.../launch/move_to_start.launch | 7 +----
...e_config.yaml => franka_gripper_node.yaml} | 0
franka_gripper/launch/franka_gripper.launch | 12 ++++----
.../launch/franka_visualization.launch | 2 --
panda_moveit_config/launch/moveit_rviz.launch | 12 +++-----
.../launch/panda_moveit.launch | 7 ++---
12 files changed, 52 insertions(+), 53 deletions(-)
rename franka_control/config/{franka_control.yaml => franka_control_node.yaml} (100%)
rename franka_gripper/config/{gripper_node_config.yaml => franka_gripper_node.yaml} (100%)
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 12045cb..9109c47 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 e3a3f94..533a31c 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 1b2fe02..dbc6d19 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 ac44266..6192686 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 2faa5ae..f7af454 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 ed8accb..15c8f66 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 d990574..93c9e43 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 a713f4f..4bbee5d 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 71f196e..17387f9 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 a879467..cab708e 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>
--
GitLab