robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

➕ add test MoveitCpp node

Browse source
This commit is contained in:
Ilya Uraev 2021-11-03 15:18:12 +04:00
parent b2a89ecbde
commit 554a33fc43
15 changed files with 653 additions and 430 deletions
Download patch file Download diff file Expand all files Collapse all files

71
rasms_moveit_config/launch/rasms_moveit_interface.launch.py
View file

@ -63,7 +63,7 @@ def launch_setup(context, *args, **kwargs):
}
# Planning
ompl_yaml = load_yaml("rasms_moveit_config", "config/ompl_planning.yml")
#ompl_yaml = load_yaml("rasms_moveit_config", "config/ompl_planning.yml")
planning = {
"move_group": {
@ -78,49 +78,79 @@ def launch_setup(context, *args, **kwargs):
"moveit_manage_controllers": True}
# Controllers
controllers_yaml = load_yaml(
moveit_simple_controller_yml = load_yaml(
LaunchConfiguration("moveit_controller_configurations_package").perform(context),
LaunchConfiguration("moveit_controller_configurations").perform(context)
)
moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml,
moveit_controllers = {"moveit_simple_controller_manager": moveit_simple_controller_yml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}
ompl_planning_pipeline_config = {
"ompl": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yml = load_yaml(
"rasms_moveit_config", "config/ompl_planning.yml"
)
ompl_planning_pipeline_config["ompl"].update(ompl_planning_yml)
# Planning scene
planning_scene_monitor_parameters = {"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True}
moveit_cpp_yaml_file_name = (
get_package_share_directory("rasms_moveit_config") + "/config/moveit_cpp.yml"
)
# Time configuration
use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}
# Prepare move group node
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
#"""move_group_node = Node(
# package="moveit_ros_move_group",
# executable="move_group",
# output="screen",
# arguments=["--ros-args"],
# parameters=[
# robot_description,
# robot_description_semantic,
# kinematics_yaml,
# robot_description_planning,
# ompl_yaml,
# planning,
# trajectory_execution,
# moveit_controllers,
# planning_scene_monitor_parameters,
# use_sim_time
# ]
#)"""
moveit_cpp_node = Node(
name="moveit_cpp_actions",
package="rasms_moveit_actions",
executable="rasms_moveit",
output="screen",
arguments=["--ros-args"],
parameters=[
moveit_cpp_yaml_file_name,
robot_description,
robot_description_semantic,
kinematics_yaml,
robot_description_planning,
ompl_yaml,
planning,
trajectory_execution,
ompl_planning_pipeline_config,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time
]
],
)
robot_state_publisher = Node(
"""robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description]
)
)"""
spawn_controller = Node(
package="controller_manager",
@ -130,7 +160,7 @@ def launch_setup(context, *args, **kwargs):
)
# RViz
rviz_config = PathJoinSubstitution([FindPackageShare("rasms_moveit_config"), "config/robasm_sgonov.rviz"])
rviz_config = PathJoinSubstitution([FindPackageShare("rasms_moveit_config"), "config/rasms.rviz"])
rviz = Node(
package="rviz2",
@ -138,8 +168,6 @@ def launch_setup(context, *args, **kwargs):
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
planning,
use_sim_time
],
arguments=[
@ -148,9 +176,10 @@ def launch_setup(context, *args, **kwargs):
)
return [
move_group_node,
#move_group_node,
moveit_cpp_node,
rviz,
robot_state_publisher,
#robot_state_publisher,
spawn_controller
]