Skip to content

Commit c350bfc

Browse files
author
Darby Lim
committed
moveit launchs
1 parent cdd6124 commit c350bfc

File tree

10 files changed

+391
-61
lines changed

10 files changed

+391
-61
lines changed

‎turtlebot3_manipulation_moveit_config/launch/move_group.launch.py‎

Lines changed: 130 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,136 @@
1616
#
1717
# Authors: Hye-jong KIM
1818

19-
from moveit_configs_utils import MoveItConfigsBuilder
20-
from moveit_configs_utils.launches import generate_move_group_launch
19+
# setup assistant (humble)
20+
# from moveit_configs_utils import MoveItConfigsBuilder
21+
# from moveit_configs_utils.launches import generate_move_group_launch
22+
# def generate_launch_description():
23+
# moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
24+
# package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
25+
# return generate_move_group_launch(moveit_config)
26+
27+
28+
import os
29+
import yaml
30+
import xacro
31+
32+
from launch import LaunchDescription
33+
from launch.actions import DeclareLaunchArgument
34+
from launch.substitutions import LaunchConfiguration
35+
from launch_ros.actions import Node
36+
from ament_index_python.packages import get_package_share_directory
2137

2238

2339
def generate_launch_description():
24-
moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
25-
package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
26-
return generate_move_group_launch(moveit_config)
40+
# Robot description
41+
robot_description_config = xacro.process_file(
42+
os.path.join(
43+
get_package_share_directory("turtlebot3_manipulation_description"),
44+
"urdf",
45+
"turtlebot3_manipulation.urdf.xacro",
46+
)
47+
)
48+
robot_description = {"robot_description": robot_description_config.toxml()}
49+
50+
# Robot description Semantic config
51+
robot_description_semantic_path = os.path.join(
52+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
53+
"config",
54+
"turtlebot3_manipulation.srdf",
55+
)
56+
with open(robot_description_semantic_path, "r") as file:
57+
robot_description_semantic_config = file.read()
58+
59+
robot_description_semantic = {
60+
"robot_description_semantic": robot_description_semantic_config
61+
}
62+
63+
# kinematics yaml
64+
kinematics_yaml_path = os.path.join(
65+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
66+
"config",
67+
"kinematics.yaml",
68+
)
69+
with open(kinematics_yaml_path, "r") as file:
70+
kinematics_yaml = yaml.safe_load(file)
71+
72+
# Planning Functionality
73+
# Planning Functionality
74+
ompl_planning_pipeline_config = {
75+
"move_group": {
76+
"planning_plugin": "ompl_interface/OMPLPlanner",
77+
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization \
78+
default_planner_request_adapters/FixWorkspaceBounds \
79+
default_planner_request_adapters/FixStartStateBounds \
80+
default_planner_request_adapters/FixStartStateCollision \
81+
default_planner_request_adapters/FixStartStatePathConstraints""",
82+
"start_state_max_bounds_error": 0.1,
83+
}
84+
}
85+
ompl_planning_yaml_path = os.path.join(
86+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
87+
"config",
88+
"ompl_planning.yaml",
89+
)
90+
with open(ompl_planning_yaml_path, "r") as file:
91+
ompl_planning_yaml = yaml.safe_load(file)
92+
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
93+
94+
# Trajectory Execution
95+
trajectory_execution = {
96+
"moveit_manage_controllers": True,
97+
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
98+
"trajectory_execution.allowed_goal_duration_margin": 0.5,
99+
"trajectory_execution.allowed_start_tolerance": 0.01,
100+
}
101+
102+
# Moveit Controllers
103+
moveit_simple_controllers_yaml_path = os.path.join(
104+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
105+
"config",
106+
"moveit_controllers.yaml",
107+
)
108+
with open(moveit_simple_controllers_yaml_path, "r") as file:
109+
moveit_simple_controllers_yaml = yaml.safe_load(file)
110+
111+
moveit_controllers = {
112+
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
113+
"moveit_controller_manager":
114+
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
115+
}
116+
117+
# Planning Scene Monitor Parameters
118+
planning_scene_monitor_parameters = {
119+
"publish_planning_scene": True,
120+
"publish_geometry_updates": True,
121+
"publish_state_updates": True,
122+
"publish_transforms_updates": True,
123+
}
124+
125+
ld = LaunchDescription()
126+
use_sim = LaunchConfiguration('use_sim')
127+
declare_use_sim = DeclareLaunchArgument(
128+
'use_sim',
129+
default_value='false',
130+
description='Start robot in Gazebo simulation.')
131+
ld.add_action(declare_use_sim)
132+
133+
move_group_node = Node(
134+
package="moveit_ros_move_group",
135+
executable="move_group",
136+
output="screen",
137+
parameters=[
138+
robot_description,
139+
robot_description_semantic,
140+
kinematics_yaml,
141+
ompl_planning_pipeline_config,
142+
trajectory_execution,
143+
moveit_controllers,
144+
planning_scene_monitor_parameters,
145+
{'use_sim_time': use_sim},
146+
],
147+
)
148+
149+
ld.add_action(move_group_node)
150+
151+
return ld

‎turtlebot3_manipulation_moveit_config/launch/moveit_fake.launch.py‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,14 @@
1616
#
1717
# Authors: Hye-jong KIM
1818

19+
# setup assistant (humble)
20+
# from moveit_configs_utils import MoveItConfigsBuilder
21+
# from moveit_configs_utils.launches import generate_demo_launch
22+
# def generate_launch_description():
23+
# moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
24+
# package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
25+
# return generate_demo_launch(moveit_config)
26+
1927
import os
2028

2129
from launch import LaunchDescription

‎turtlebot3_manipulation_moveit_config/launch/moveit_gazebo.launch.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ def generate_launch_description():
6666
'empty_world.model'
6767
]
6868
)
69+
6970
gazebo_control_launch = IncludeLaunchDescription(
7071
PythonLaunchDescriptionSource([bringup_launch_dir, '/gazebo.launch.py']),
7172
launch_arguments={

‎turtlebot3_manipulation_moveit_config/launch/moveit_rviz.launch.py‎

Lines changed: 94 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,100 @@
1717
# Authors: Hye-jong KIM
1818

1919
# setup assistant (humble)
20-
from moveit_configs_utils import MoveItConfigsBuilder
21-
from moveit_configs_utils.launches import generate_moveit_rviz_launch
20+
# from moveit_configs_utils import MoveItConfigsBuilder
21+
# from moveit_configs_utils.launches import generate_moveit_rviz_launch
22+
# def generate_launch_description():
23+
# moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
24+
# package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
25+
# return generate_moveit_rviz_launch(moveit_config)
26+
27+
import os
28+
import xacro
29+
import yaml
30+
31+
from ament_index_python.packages import get_package_share_directory
32+
from launch import LaunchDescription
33+
from launch_ros.actions import Node
2234

2335

2436
def generate_launch_description():
25-
moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
26-
package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
27-
return generate_moveit_rviz_launch(moveit_config)
37+
38+
# Rviz config save file
39+
rviz_config = os.path.join(
40+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
41+
"config",
42+
"moveit.rviz"
43+
)
44+
45+
# Robot description
46+
robot_description_config = xacro.process_file(
47+
os.path.join(
48+
get_package_share_directory("turtlebot3_manipulation_description"),
49+
"urdf",
50+
"turtlebot3_manipulation.urdf.xacro",
51+
)
52+
)
53+
robot_description = {"robot_description": robot_description_config.toxml()}
54+
55+
# Robot description Semantic config
56+
robot_description_semantic_path = os.path.join(
57+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
58+
"config",
59+
"turtlebot3_manipulation.srdf",
60+
)
61+
with open(robot_description_semantic_path, "r") as file:
62+
robot_description_semantic_config = file.read()
63+
64+
robot_description_semantic = {
65+
"robot_description_semantic": robot_description_semantic_config
66+
}
67+
68+
# Planning Functionality
69+
ompl_planning_pipeline_config = {
70+
"move_group": {
71+
"planning_plugin": "ompl_interface/OMPLPlanner",
72+
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization \
73+
default_planner_request_adapters/FixWorkspaceBounds \
74+
default_planner_request_adapters/FixStartStateBounds \
75+
default_planner_request_adapters/FixStartStateCollision \
76+
default_planner_request_adapters/FixStartStatePathConstraints""",
77+
"start_state_max_bounds_error": 0.1,
78+
}
79+
}
80+
ompl_planning_yaml_path = os.path.join(
81+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
82+
"config",
83+
"ompl_planning.yaml",
84+
)
85+
with open(ompl_planning_yaml_path, "r") as file:
86+
ompl_planning_yaml = yaml.safe_load(file)
87+
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
88+
89+
# kinematics yaml
90+
kinematics_yaml_path = os.path.join(
91+
get_package_share_directory("turtlebot3_manipulation_moveit_config"),
92+
"config",
93+
"kinematics.yaml",
94+
)
95+
with open(kinematics_yaml_path, "r") as file:
96+
kinematics_yaml = yaml.safe_load(file)
97+
98+
ld = LaunchDescription()
99+
100+
rviz_node = Node(
101+
package="rviz2",
102+
executable="rviz2",
103+
name="rviz2",
104+
output="log",
105+
arguments=["-d", rviz_config],
106+
parameters=[
107+
robot_description,
108+
robot_description_semantic,
109+
ompl_planning_pipeline_config,
110+
kinematics_yaml,
111+
]
112+
)
113+
114+
ld.add_action(rviz_node)
115+
116+
return ld

‎turtlebot3_manipulation_moveit_config/launch/rsp.launch.py‎

Lines changed: 43 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,49 @@
1616
#
1717
# Authors: Hye-jong KIM
1818

19-
from moveit_configs_utils import MoveItConfigsBuilder
20-
from moveit_configs_utils.launches import generate_rsp_launch
19+
# setup assistant (humble)
20+
# from moveit_configs_utils import MoveItConfigsBuilder
21+
# from moveit_configs_utils.launches import generate_rsp_launch
22+
# def generate_launch_description():
23+
# moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
24+
# package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
25+
# return generate_rsp_launch(moveit_config)
26+
27+
import os
28+
import xacro
29+
30+
from ament_index_python.packages import get_package_share_directory
31+
from launch import LaunchDescription
32+
from launch.actions import DeclareLaunchArgument
33+
from launch.substitutions import LaunchConfiguration
34+
from launch_ros.actions import Node
2135

2236

2337
def generate_launch_description():
24-
moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
25-
package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
26-
return generate_rsp_launch(moveit_config)
27-
38+
39+
ld = LaunchDescription()
40+
publish_frequency = LaunchConfiguration("publish_frequency")
41+
ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))
42+
43+
# Robot description
44+
robot_description_config = xacro.process_file(
45+
os.path.join(
46+
get_package_share_directory("turtlebot3_manipulation_description"),
47+
"urdf",
48+
"turtlebot3_manipulation.urdf.xacro",
49+
)
50+
)
51+
robot_description = {"robot_description": robot_description_config.toxml()}
52+
53+
# Given the published joint states, publish tf for the robot links and the robot description
54+
rsp_node = Node(
55+
package="robot_state_publisher",
56+
executable="robot_state_publisher",
57+
respawn=True,
58+
output="screen",
59+
parameters=[{"publish_frequency": publish_frequency}, robot_description]
60+
)
61+
62+
ld.add_action(rsp_node)
63+
64+
return ld

‎turtlebot3_manipulation_moveit_config/launch/servo.launch.py‎

Lines changed: 11 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,7 @@
2121
from launch import LaunchDescription
2222
from launch.actions import DeclareLaunchArgument
2323
from launch.substitutions import LaunchConfiguration
24-
from launch_ros.actions import ComposableNodeContainer
25-
from launch_ros.descriptions import ComposableNode
24+
from launch_ros.actions import Node
2625
from ament_index_python.packages import get_package_share_directory
2726
import xacro
2827

@@ -77,27 +76,16 @@ def generate_launch_description():
7776
return None
7877

7978
# Launch as much as possible in components
80-
container = ComposableNodeContainer(
81-
name="turtlebot_manipulation_moveit_servo_container",
82-
namespace="/",
83-
package="rclcpp_components",
84-
executable="component_container",
85-
composable_node_descriptions=[
86-
ComposableNode(
87-
package="moveit_servo",
88-
plugin="moveit_servo::ServoServer",
89-
name="servo_server",
90-
parameters=[
91-
{'use_gazebo': use_sim},
92-
servo_params,
93-
robot_description,
94-
robot_description_semantic,
95-
],
96-
extra_arguments=[{"use_intra_process_comms": True}],
97-
),
98-
],
99-
output="screen",
79+
servo_node = Node(
80+
package="moveit_servo",
81+
executable="servo_node_main",
82+
parameters=[
83+
{'use_gazebo':use_sim}
84+
servo_params,
85+
robot_description,
86+
robot_description_semantic,
87+
]
10088
)
101-
ld.add_action(container)
89+
ld.add_action(servo_node)
10290

10391
return ld

‎turtlebot3_manipulation_moveit_config/launch/setup_assistant.launch.py‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,12 @@
1616
#
1717
# Authors: Hye-jong KIM
1818

19-
from moveit_configs_utils import MoveItConfigsBuilder
20-
from moveit_configs_utils.launches import generate_setup_assistant_launch
19+
# setup assistant (humble)
20+
# from moveit_configs_utils import MoveItConfigsBuilder
21+
# from moveit_configs_utils.launches import generate_setup_assistant_launch
22+
# def generate_launch_description():
23+
# moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
24+
# package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
25+
# return generate_setup_assistant_launch(moveit_config)
2126

22-
23-
def generate_launch_description():
24-
moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation",
25-
package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs()
26-
return generate_setup_assistant_launch(moveit_config)
27+
# moveit setup assistant is not supported in ros foxy version.

0 commit comments

Comments
 (0)