-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsingle_joint_example.launch
93 lines (79 loc) · 3.86 KB
/
single_joint_example.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- ================= -->
<!-- Robot description -->
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description" textfile="$(find layered_hardware_gazebo)/urdf/single_joint_example.urdf" />
<!-- ========================================= -->
<!-- Virtual robot & world in Gazebo simulator -->
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find layered_hardware_gazebo)/world/single_joint_example.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model single_joint_example -param robot_description -z 1"/>
<!-- ========================================= -->
<!-- Embedded controllers on the virtual robot -->
<rosparam>
single_joint_example:
layered_hardware_gazebo:
control_frequency: 10.
# upper (controller-side) to bottom (hardware-side)
layers: [ joint_limits_layer, gazebo_joint_layer ]
joint_limits_layer:
type: layered_hardware_extensions/ExtendedJointLimitsLayer
gazebo_joint_layer:
type: layered_hardware_gazebo/GazeboJointLayer
joints:
axis:
operation_mode_map:
posvel_controller: posvel
position_controller: position_pid
velocity_controller: velocity
effort_controller: effort
position_pid:
p: 10.
i: 0.
d: 2.
publish_state: false
initial_position: 0.785
</rosparam>
<!-- ==================== -->
<!-- ROS-side controllers -->
<group ns="single_joint_example">
<!-- Controller params -->
<rosparam>
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 20
posvel_controller:
type: posvel_controllers/JointPosVelController
joint: axis
position_controller:
type: position_controllers/JointPositionController
joint: axis
velocity_controller:
type: velocity_controllers/JointVelocityController
joint: axis
effort_controller:
type: effort_controllers/JointEffortController
joint: axis
</rosparam>
<!-- Controller starter -->
<node name="controller_starter" pkg="controller_manager" type="controller_manager" output="screen"
args="spawn joint_state_controller position_controller" />
<node name="controller_loader" pkg="controller_manager" type="controller_manager" output="screen"
args="load posvel_controller velocity_controller effort_controller" />
</group>
</launch>