Skip to content

Commit 5bcb5b6

Browse files
mcbedmcbed
authored
Merge pull request #1 from ICube-Robotics/mcbed-setup-description
ramsai description and planning
2 parents a94a5b5 + a5bd834 commit 5bcb5b6

32 files changed

+6306
-1
lines changed

‎ramsai/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
<version>0.0.1</version>
66
<description>Meta-package aggregating the ramsai packages and documentation</description>
77
<maintainer email="[email protected]">Laurent Barbé</maintainer>
8-
<maintainer email="macbednarczyk@gmail.com">Maciej Bednarczyk</maintainer>
8+
<maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
99
<license>Apache License 2.0</license>
1010

1111
<buildtool_depend>ament_cmake</buildtool_depend>

‎ramsai_bringup/CMakeLists.txt‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(ramsai_bringup)
3+
4+
# Default to C99
5+
if(NOT CMAKE_C_STANDARD)
6+
set(CMAKE_C_STANDARD 99)
7+
endif()
8+
9+
# Default to C++14
10+
if(NOT CMAKE_CXX_STANDARD)
11+
set(CMAKE_CXX_STANDARD 14)
12+
endif()
13+
14+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
15+
add_compile_options(-Wall -Wextra -Wpedantic)
16+
endif()
17+
18+
# find dependencies
19+
find_package(ament_cmake REQUIRED)
20+
find_package(rclcpp REQUIRED)
21+
find_package(rclpy REQUIRED)
22+
find_package(sensor_msgs REQUIRED)
23+
find_package(visualization_msgs REQUIRED)
24+
25+
if(BUILD_TESTING)
26+
find_package(ament_cmake_gtest REQUIRED)
27+
find_package(ament_lint_auto REQUIRED)
28+
ament_lint_auto_find_test_dependencies()
29+
endif()
30+
31+
################################################################################
32+
# Install
33+
################################################################################
34+
install(
35+
DIRECTORY launch
36+
DESTINATION share/${PROJECT_NAME}
37+
)
38+
39+
ament_package()

‎ramsai_bringup/launch/ramsai.launch.py‎

Lines changed: 298 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,298 @@
1+
# Copyright 2022 ICube Laboratory, University of Strasbourg
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from launch import LaunchDescription
16+
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
17+
from launch.conditions import IfCondition, UnlessCondition
18+
from launch.event_handlers import OnProcessExit, OnProcessStart
19+
from launch.launch_description_sources import PythonLaunchDescriptionSource
20+
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
21+
from launch_ros.actions import Node
22+
from launch_ros.substitutions import FindPackageShare
23+
24+
25+
def generate_launch_description():
26+
# Declare arguments
27+
declared_arguments = []
28+
declared_arguments.append(
29+
DeclareLaunchArgument(
30+
'use_sim',
31+
default_value='false',
32+
description='Start robot in Gazebo simulation.',
33+
)
34+
)
35+
declared_arguments.append(
36+
DeclareLaunchArgument(
37+
'use_fake_hardware',
38+
default_value='true',
39+
description='Start robot with fake hardware mirroring command to its states.',
40+
)
41+
)
42+
declared_arguments.append(
43+
DeclareLaunchArgument(
44+
'use_planning',
45+
default_value='false',
46+
description='Start robot with Moveit2 `move_group` planning \
47+
config for Pilz and OMPL.',
48+
)
49+
)
50+
declared_arguments.append(
51+
DeclareLaunchArgument(
52+
'use_servoing',
53+
default_value='false',
54+
description='Start robot with Moveit2 servoing.',
55+
)
56+
)
57+
declared_arguments.append(
58+
DeclareLaunchArgument(
59+
'robot_controller',
60+
default_value='iiwa_arm_controller',
61+
description='Robot controller to start.',
62+
)
63+
)
64+
declared_arguments.append(
65+
DeclareLaunchArgument(
66+
'start_rviz',
67+
default_value='true',
68+
description='Start RViz2 automatically with this launch file.',
69+
)
70+
)
71+
declared_arguments.append(
72+
DeclareLaunchArgument(
73+
'robot_ip',
74+
default_value='192.170.10.2',
75+
description='Robot IP of FRI interface',
76+
)
77+
)
78+
declared_arguments.append(
79+
DeclareLaunchArgument(
80+
'robot_port',
81+
default_value='30200',
82+
description='Robot port of FRI interface.',
83+
)
84+
)
85+
declared_arguments.append(
86+
DeclareLaunchArgument(
87+
'initial_positions_file',
88+
default_value='initial_positions.yaml',
89+
description='Configuration file of robot initial positions for simulation.',
90+
)
91+
)
92+
declared_arguments.append(
93+
DeclareLaunchArgument(
94+
'command_interface',
95+
default_value='position',
96+
description='Robot command interface [position|velocity|effort].',
97+
)
98+
)
99+
declared_arguments.append(
100+
DeclareLaunchArgument(
101+
'base_frame_file',
102+
default_value='base_frame.yaml',
103+
description='Configuration file of robot base frame wrt World.',
104+
)
105+
)
106+
107+
# Initialize Arguments
108+
use_sim = LaunchConfiguration('use_sim')
109+
use_fake_hardware = LaunchConfiguration('use_fake_hardware')
110+
use_planning = LaunchConfiguration('use_planning')
111+
robot_controller = LaunchConfiguration('robot_controller')
112+
start_rviz = LaunchConfiguration('start_rviz')
113+
robot_ip = LaunchConfiguration('robot_ip')
114+
robot_port = LaunchConfiguration('robot_port')
115+
initial_positions_file = LaunchConfiguration('initial_positions_file')
116+
command_interface = LaunchConfiguration('command_interface')
117+
base_frame_file = LaunchConfiguration('base_frame_file')
118+
119+
# Get URDF via xacro
120+
robot_description_content = Command(
121+
[
122+
PathJoinSubstitution([FindExecutable(name='xacro')]),
123+
' ',
124+
PathJoinSubstitution(
125+
[FindPackageShare('ramsai_description'), 'config', 'ramsai.config.xacro']
126+
),
127+
' ',
128+
'prefix:=""',
129+
' ',
130+
'use_sim:=',
131+
use_sim,
132+
' ',
133+
'use_fake_hardware:=',
134+
use_fake_hardware,
135+
' ',
136+
'robot_ip:=',
137+
robot_ip,
138+
' ',
139+
'robot_port:=',
140+
robot_port,
141+
' ',
142+
'initial_positions_file:=',
143+
initial_positions_file,
144+
' ',
145+
'command_interface:=',
146+
command_interface,
147+
' ',
148+
'base_frame_file:=',
149+
base_frame_file,
150+
]
151+
)
152+
153+
robot_description = {'robot_description': robot_description_content}
154+
155+
# Running with Moveit2 planning
156+
iiwa_planning_launch = IncludeLaunchDescription(
157+
PythonLaunchDescriptionSource([
158+
FindPackageShare('ramsai_bringup'),
159+
'/launch',
160+
'/ramsai_planning.launch.py'
161+
]),
162+
launch_arguments={
163+
'start_rviz': start_rviz,
164+
'base_frame_file': base_frame_file,
165+
'use_sim': use_sim,
166+
}.items(),
167+
condition=IfCondition(use_planning),
168+
)
169+
170+
robot_controllers = PathJoinSubstitution(
171+
[
172+
FindPackageShare('iiwa_description'),
173+
'config',
174+
'iiwa_controllers.yaml',
175+
]
176+
)
177+
rviz_config_file = PathJoinSubstitution(
178+
[FindPackageShare('iiwa_description'), 'rviz', 'iiwa.rviz']
179+
)
180+
181+
control_node = Node(
182+
package='controller_manager',
183+
executable='ros2_control_node',
184+
parameters=[robot_description, robot_controllers],
185+
output='both',
186+
condition=UnlessCondition(use_sim),
187+
)
188+
robot_state_pub_node = Node(
189+
package='robot_state_publisher',
190+
executable='robot_state_publisher',
191+
output='both',
192+
parameters=[robot_description],
193+
)
194+
rviz_node = Node(
195+
package='rviz2',
196+
executable='rviz2',
197+
name='rviz2',
198+
output='log',
199+
arguments=['-d', rviz_config_file],
200+
parameters=[
201+
robot_description,
202+
],
203+
condition=UnlessCondition(use_planning),
204+
)
205+
iiwa_simulation_world = PathJoinSubstitution(
206+
[FindPackageShare('iiwa_description'),
207+
'gazebo/worlds', 'empty.world']
208+
)
209+
210+
gazebo = IncludeLaunchDescription(
211+
PythonLaunchDescriptionSource(
212+
[PathJoinSubstitution(
213+
[FindPackageShare('gazebo_ros'),
214+
'launch', 'gazebo.launch.py']
215+
)]
216+
),
217+
launch_arguments={'verbose': 'false', 'world': iiwa_simulation_world}.items(),
218+
condition=IfCondition(use_sim),
219+
)
220+
221+
spawn_entity = Node(
222+
package='gazebo_ros',
223+
executable='spawn_entity.py',
224+
arguments=['-topic', ['robot_description'], '-entity', ['iiwa14']],
225+
output='screen',
226+
condition=IfCondition(use_sim),
227+
)
228+
229+
joint_state_broadcaster_spawner = Node(
230+
package='controller_manager',
231+
executable='spawner',
232+
arguments=['joint_state_broadcaster', '--controller-manager',
233+
['controller_manager']],
234+
)
235+
236+
external_torque_broadcaster_spawner = Node(
237+
package='controller_manager',
238+
executable='spawner',
239+
arguments=['ets_state_broadcaster', '--controller-manager',
240+
['controller_manager']],
241+
condition=UnlessCondition(use_sim),
242+
)
243+
244+
robot_controller_spawner = Node(
245+
package='controller_manager',
246+
executable='spawner',
247+
arguments=[robot_controller, '--controller-manager', ['controller_manager']],
248+
)
249+
250+
# Delay `joint_state_broadcaster` after spawn_entity
251+
delay_joint_state_broadcaster_spawner_after_spawn_entity = RegisterEventHandler(
252+
event_handler=OnProcessExit(
253+
target_action=spawn_entity,
254+
on_exit=[joint_state_broadcaster_spawner],
255+
),
256+
condition=IfCondition(use_sim),
257+
)
258+
259+
# Delay `joint_state_broadcaster` after control_node
260+
delay_joint_state_broadcaster_spawner_after_control_node = RegisterEventHandler(
261+
event_handler=OnProcessStart(
262+
target_action=control_node,
263+
on_start=[joint_state_broadcaster_spawner],
264+
),
265+
condition=UnlessCondition(use_sim),
266+
)
267+
268+
# Delay rviz start after `joint_state_broadcaster`
269+
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
270+
event_handler=OnProcessExit(
271+
target_action=joint_state_broadcaster_spawner,
272+
on_exit=[rviz_node],
273+
),
274+
condition=IfCondition(start_rviz),
275+
)
276+
277+
# Delay start of robot_controller after `joint_state_broadcaster`
278+
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
279+
event_handler=OnProcessExit(
280+
target_action=joint_state_broadcaster_spawner,
281+
on_exit=[robot_controller_spawner],
282+
)
283+
)
284+
285+
nodes = [
286+
gazebo,
287+
control_node,
288+
iiwa_planning_launch,
289+
spawn_entity,
290+
robot_state_pub_node,
291+
delay_joint_state_broadcaster_spawner_after_control_node,
292+
delay_joint_state_broadcaster_spawner_after_spawn_entity,
293+
delay_rviz_after_joint_state_broadcaster_spawner,
294+
external_torque_broadcaster_spawner,
295+
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
296+
]
297+
298+
return LaunchDescription(declared_arguments + nodes)

0 commit comments

Comments
 (0)