Skip to content

Commit 81aa932

Browse files
sjahrsjahr
committed
Initial commit for planner cost function tutorial
1 parent 1834b79 commit 81aa932

File tree

6 files changed

+464
-0
lines changed

6 files changed

+464
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning)
6969
add_subdirectory(doc/how_to_guides/chomp_planner)
7070
add_subdirectory(doc/how_to_guides/persistent_scenes_and_states)
7171
add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner)
72+
add_subdirectory(doc/how_to_guides/planner_cost_functions)
7273
add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
7374
add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
7475
add_subdirectory(doc/tutorials/quickstart_in_rviz)

‎doc/how_to_guides/planner_cost_functions/CMakeLists.txt‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
add_executable(planner_cost_functions_example src/planner_cost_functions_main.cpp)
2+
target_include_directories(planner_cost_functions_example PRIVATE include)
3+
ament_target_dependencies(planner_cost_functions_example ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
4+
5+
install(TARGETS planner_cost_functions_example
6+
DESTINATION lib/${PROJECT_NAME}
7+
)
8+
install(DIRECTORY launch
9+
DESTINATION share/${PROJECT_NAME}
10+
)
11+
install(DIRECTORY config
12+
DESTINATION share/${PROJECT_NAME}
13+
)

‎doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
# Default configurations. More information can be found in the moveit_cpp and planning scene monitor tutorials
2+
planning_scene_monitor_options:
3+
name: "planning_scene_monitor"
4+
robot_description: "robot_description"
5+
joint_state_topic: "/joint_states"
6+
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
7+
publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
8+
monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
9+
wait_for_initial_state_timeout: 10.0
10+
11+
# Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning
12+
planning_pipelines:
13+
pipeline_names: ["ompl"]
14+
15+
plan_request_params:
16+
planning_attempts: 1
17+
planning_pipeline: ompl # Different OMPL pipeline name!
18+
planner_id: "RRTstarkConfigDefault"
19+
max_velocity_scaling_factor: 1.0
20+
max_acceleration_scaling_factor: 1.0
21+
planning_time: 1.5

‎doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py‎

Lines changed: 171 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
1+
import os
2+
import yaml
3+
from launch import LaunchDescription
4+
from launch.actions import DeclareLaunchArgument, OpaqueFunction
5+
from launch_ros.actions import Node
6+
from ament_index_python.packages import get_package_share_directory
7+
from moveit_configs_utils import MoveItConfigsBuilder
8+
9+
10+
def load_yaml(package_name, file_path):
11+
package_path = get_package_share_directory(package_name)
12+
absolute_file_path = os.path.join(package_path, file_path)
13+
14+
try:
15+
with open(absolute_file_path, "r") as file:
16+
return yaml.safe_load(file)
17+
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
18+
return None
19+
20+
21+
def generate_launch_description():
22+
23+
declared_arguments = []
24+
declared_arguments.append(
25+
DeclareLaunchArgument(
26+
"rviz_config",
27+
default_value="panda_moveit_config_demo.rviz",
28+
description="RViz configuration file",
29+
)
30+
)
31+
32+
return LaunchDescription(
33+
declared_arguments + [OpaqueFunction(function=launch_setup)]
34+
)
35+
36+
37+
def launch_setup(context, *args, **kwargs):
38+
39+
moveit_config = (
40+
MoveItConfigsBuilder("moveit_resources_panda")
41+
.robot_description(file_path="config/panda.urdf.xacro")
42+
.planning_scene_monitor(
43+
publish_robot_description=True, publish_robot_description_semantic=True
44+
)
45+
.planning_pipelines("ompl", ["ompl"])
46+
.trajectory_execution(file_path="config/moveit_controllers.yaml")
47+
.moveit_cpp(
48+
os.path.join(
49+
get_package_share_directory("moveit2_tutorials"),
50+
"config",
51+
"planner_cost_moveit_cpp.yaml",
52+
)
53+
)
54+
.to_moveit_configs()
55+
)
56+
57+
# Warehouse config
58+
sqlite_database = os.path.join(
59+
get_package_share_directory("moveit2_tutorials"),
60+
"config",
61+
"panda_test_db.sqlite",
62+
)
63+
64+
warehouse_ros_config = {
65+
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
66+
"warehouse": {
67+
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
68+
"host": sqlite_database,
69+
"port": 33828,
70+
"scene_name": "kitchen_panda_scene_sensed1",
71+
},
72+
}
73+
74+
# MoveItCpp demo executable
75+
moveit_cpp_node = Node(
76+
name="planner_cost_functions_example",
77+
package="moveit2_tutorials",
78+
executable="planner_cost_functions_example",
79+
output="screen",
80+
parameters=[moveit_config.to_dict(), warehouse_ros_config],
81+
)
82+
83+
# RViz
84+
rviz_config_file = os.path.join(
85+
get_package_share_directory("moveit2_tutorials"),
86+
"config",
87+
"parallel_planning_config.rviz",
88+
)
89+
90+
rviz_node = Node(
91+
package="rviz2",
92+
executable="rviz2",
93+
name="rviz2",
94+
output="log",
95+
arguments=["-d", rviz_config_file],
96+
parameters=[
97+
moveit_config.robot_description,
98+
moveit_config.robot_description_semantic,
99+
moveit_config.robot_description_kinematics,
100+
moveit_config.planning_pipelines,
101+
moveit_config.joint_limits,
102+
],
103+
)
104+
105+
# Static TF
106+
static_tf = Node(
107+
package="tf2_ros",
108+
executable="static_transform_publisher",
109+
name="static_transform_publisher",
110+
output="log",
111+
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
112+
)
113+
114+
# Publish TF
115+
robot_state_publisher = Node(
116+
package="robot_state_publisher",
117+
executable="robot_state_publisher",
118+
name="robot_state_publisher",
119+
output="both",
120+
parameters=[moveit_config.robot_description],
121+
)
122+
123+
# ros2_control using FakeSystem as hardware
124+
ros2_controllers_path = os.path.join(
125+
get_package_share_directory("moveit_resources_panda_moveit_config"),
126+
"config",
127+
"ros2_controllers.yaml",
128+
)
129+
ros2_control_node = Node(
130+
package="controller_manager",
131+
executable="ros2_control_node",
132+
parameters=[moveit_config.robot_description, ros2_controllers_path],
133+
output="both",
134+
)
135+
136+
joint_state_broadcaster_spawner = Node(
137+
package="controller_manager",
138+
executable="spawner",
139+
arguments=[
140+
"joint_state_broadcaster",
141+
"--controller-manager-timeout",
142+
"300",
143+
"--controller-manager",
144+
"/controller_manager",
145+
],
146+
)
147+
148+
arm_controller_spawner = Node(
149+
package="controller_manager",
150+
executable="spawner",
151+
arguments=["panda_arm_controller", "-c", "/controller_manager"],
152+
)
153+
154+
hand_controller_spawner = Node(
155+
package="controller_manager",
156+
executable="spawner",
157+
arguments=["panda_hand_controller", "-c", "/controller_manager"],
158+
)
159+
160+
nodes_to_start = [
161+
static_tf,
162+
robot_state_publisher,
163+
rviz_node,
164+
moveit_cpp_node,
165+
ros2_control_node,
166+
joint_state_broadcaster_spawner,
167+
arm_controller_spawner,
168+
hand_controller_spawner,
169+
]
170+
171+
return nodes_to_start

‎doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
Using cost functions in the MoveIt Planning Pipeline
2+
====================================================
3+
4+
TODO

0 commit comments

Comments
 (0)