-
Notifications
You must be signed in to change notification settings - Fork 311
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Allow starting the Gazebo robot w/ and w/o a namespace.
By default, we don't start in a namespace to match the real-robot behavior. Unfortunately, we cannot just specify an empty ns="" in roslaunch, but we need to distinguish the two cases empty/non-empty namespace. To avoid code duplication, the relevant code was factored out into panda_ns.launch.xml, which is include w/ or w/o a namespace. The file-ending is deliberatively choosen as .launch.xml to hide the file from bash completion.
- Loading branch information
Showing
2 changed files
with
70 additions
and
58 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,66 @@ | ||
<?xml version="1.0"?> | ||
<launch> | ||
|
||
<!-- This is a helper launch file for panda.launch | ||
As roslaunch doesn't allow for empty ns arguments and to avoid code duplication, | ||
we factored out code here that should run either in or out a namespace. | ||
All arguments are simply inherited from the parent launch file. | ||
--> | ||
<param name="robot_description" | ||
command="xacro $(find franka_description)/robots/panda_arm.urdf.xacro | ||
gazebo:=true | ||
hand:=$(arg use_gripper) | ||
arm_id:=$(arg arm_id) | ||
ns:=$(arg ns) | ||
xyz:='$(arg x) $(arg y) $(arg z)' | ||
rpy:='$(arg roll) $(arg pitch) $(arg yaw)'"> | ||
</param> | ||
|
||
<rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" /> | ||
<rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" /> | ||
|
||
<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" /> | ||
<node name="$(arg arm_id)_model_spawner" | ||
pkg="gazebo_ros" | ||
type="spawn_model" | ||
args="-param robot_description -urdf -model $(arg arm_id) $(arg unpause) | ||
$(arg initial_joint_positions) | ||
"/> | ||
|
||
<!-- Spawn required ROS controllers --> | ||
<node pkg="controller_manager" | ||
type="spawner" | ||
name="$(arg arm_id)_gripper_spawner" | ||
if="$(arg use_gripper)" | ||
args="franka_gripper" | ||
respawn="false" | ||
/> | ||
|
||
<node pkg="controller_manager" | ||
type="spawner" | ||
name="$(arg arm_id)_controller_spawner" | ||
respawn="false" output="screen" | ||
args="franka_state_controller $(arg controller)" | ||
/> | ||
|
||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> | ||
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher"> | ||
<rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam> | ||
<param name="rate" value="30"/> | ||
</node> | ||
<node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher"> | ||
<rosparam param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam> | ||
<param name="rate" value="30"/> | ||
<remap from="joint_states" to="joint_states_desired" /> | ||
</node> | ||
|
||
<!-- Start only if cartesian_impedance_example_controller --> | ||
<node name="interactive_marker" | ||
pkg="franka_example_controllers" | ||
type="interactive_marker.py" | ||
if="$(eval arg('controller') == 'cartesian_impedance_example_controller')"> | ||
<param name="link_name" value="$(arg arm_id)_link0" /> | ||
<remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" /> | ||
</node> | ||
|
||
</launch> |