forked from moveit/moveit2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Fix move group namespace and demo.launch.py (moveit#69)
* Remove move group prefixes * Fix quickstart rviz tutorial * Fix move_group_interface and move_it_cpp tutorials * Update doc/moveit_cpp/src/moveit_cpp_tutorial.cpp * Add ros shutdown to end of tutorial * Update move_group_interface_tutorial.rst Co-authored-by: Jafar Abdi <[email protected]>
- Loading branch information
Vatan Aksoy Tezer
authored
Apr 21, 2021
1 parent
4bfbabf
commit 26b775d
Showing
24 changed files
with
1,351 additions
and
316 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -6,3 +6,5 @@ build | |
| Gemfile | ||
| native_build | ||
| *.gdb | ||
| # For local rst builds | ||
| _build/ | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,8 +1,13 @@ | ||
| add_executable(move_group_interface_tutorial | ||
| src/move_group_interface_tutorial.cpp) | ||
| target_link_libraries(move_group_interface_tutorial | ||
| ${catkin_LIBRARIES} ${Boost_LIBRARIES}) | ||
| install(TARGETS move_group_interface_tutorial DESTINATION | ||
| ${CATKIN_PACKAGE_BIN_DESTINATION}) | ||
| target_include_directories(move_group_interface_tutorial | ||
| PUBLIC include) | ||
| ament_target_dependencies(move_group_interface_tutorial | ||
| ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) | ||
|
|
||
| install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) | ||
| install(TARGETS move_group_interface_tutorial | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| ) | ||
| install(DIRECTORY launch | ||
| DESTINATION share/${PROJECT_NAME} | ||
| ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,193 @@ | ||
| import os | ||
| import yaml | ||
| from launch import LaunchDescription | ||
| from launch_ros.actions import Node | ||
| from launch.actions import ExecuteProcess | ||
| from ament_index_python.packages import get_package_share_directory | ||
| import xacro | ||
|
|
||
|
|
||
| def load_file(package_name, file_path): | ||
| package_path = get_package_share_directory(package_name) | ||
| absolute_file_path = os.path.join(package_path, file_path) | ||
|
|
||
| try: | ||
| with open(absolute_file_path, "r") as file: | ||
| return file.read() | ||
| except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available | ||
| return None | ||
|
|
||
|
|
||
| def load_yaml(package_name, file_path): | ||
| package_path = get_package_share_directory(package_name) | ||
| absolute_file_path = os.path.join(package_path, file_path) | ||
|
|
||
| try: | ||
| with open(absolute_file_path, "r") as file: | ||
| return yaml.safe_load(file) | ||
| except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available | ||
| return None | ||
|
|
||
|
|
||
| def generate_launch_description(): | ||
|
|
||
| # planning_context | ||
| robot_description_config = xacro.process_file( | ||
| os.path.join( | ||
| get_package_share_directory("moveit_resources_panda_moveit_config"), | ||
| "config", | ||
| "panda.urdf.xacro", | ||
| ) | ||
| ) | ||
| robot_description = {"robot_description": robot_description_config.toxml()} | ||
|
|
||
| robot_description_semantic_config = load_file( | ||
| "moveit_resources_panda_moveit_config", "config/panda.srdf" | ||
| ) | ||
| robot_description_semantic = { | ||
| "robot_description_semantic": robot_description_semantic_config | ||
| } | ||
|
|
||
| kinematics_yaml = load_yaml( | ||
| "moveit_resources_panda_moveit_config", "config/kinematics.yaml" | ||
| ) | ||
| robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} | ||
|
|
||
| # Planning Functionality | ||
| ompl_planning_pipeline_config = { | ||
| "move_group": { | ||
| "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_yaml = load_yaml( | ||
| "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" | ||
| ) | ||
| ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) | ||
|
|
||
| # Trajectory Execution Functionality | ||
| moveit_simple_controllers_yaml = load_yaml( | ||
| "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" | ||
| ) | ||
| moveit_controllers = { | ||
| "moveit_simple_controller_manager": moveit_simple_controllers_yaml, | ||
| "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", | ||
| } | ||
|
|
||
| trajectory_execution = { | ||
| "moveit_manage_controllers": True, | ||
| "trajectory_execution.allowed_execution_duration_scaling": 1.2, | ||
| "trajectory_execution.allowed_goal_duration_margin": 0.5, | ||
| "trajectory_execution.allowed_start_tolerance": 0.01, | ||
| } | ||
|
|
||
| planning_scene_monitor_parameters = { | ||
| "publish_planning_scene": True, | ||
| "publish_geometry_updates": True, | ||
| "publish_state_updates": True, | ||
| "publish_transforms_updates": True, | ||
| } | ||
|
|
||
| # Start the actual move_group node/action server | ||
| run_move_group_node = Node( | ||
| package="moveit_ros_move_group", | ||
| executable="move_group", | ||
| output="screen", | ||
| parameters=[ | ||
| robot_description, | ||
| robot_description_semantic, | ||
| kinematics_yaml, | ||
| ompl_planning_pipeline_config, | ||
| trajectory_execution, | ||
| moveit_controllers, | ||
| planning_scene_monitor_parameters, | ||
| ], | ||
| ) | ||
|
|
||
| # RViz | ||
| rviz_config_file = ( | ||
| get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz" | ||
| ) | ||
| rviz_node = Node( | ||
| package="rviz2", | ||
| executable="rviz2", | ||
| name="rviz2", | ||
| output="log", | ||
| arguments=["-d", rviz_config_file], | ||
| parameters=[ | ||
| robot_description, | ||
| robot_description_semantic, | ||
| ompl_planning_pipeline_config, | ||
| kinematics_yaml, | ||
| ], | ||
| ) | ||
|
|
||
| # Static TF | ||
| static_tf = Node( | ||
| package="tf2_ros", | ||
| executable="static_transform_publisher", | ||
| name="static_transform_publisher", | ||
| output="log", | ||
| arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], | ||
| ) | ||
|
|
||
| # Publish TF | ||
| robot_state_publisher = Node( | ||
| package="robot_state_publisher", | ||
| executable="robot_state_publisher", | ||
| name="robot_state_publisher", | ||
| output="both", | ||
| parameters=[robot_description], | ||
| ) | ||
|
|
||
| # ros2_control using FakeSystem as hardware | ||
| ros2_controllers_path = os.path.join( | ||
| get_package_share_directory("moveit_resources_panda_moveit_config"), | ||
| "config", | ||
| "panda_ros_controllers.yaml", | ||
| ) | ||
| ros2_control_node = Node( | ||
| package="controller_manager", | ||
| executable="ros2_control_node", | ||
| parameters=[robot_description, ros2_controllers_path], | ||
| output={ | ||
| "stdout": "screen", | ||
| "stderr": "screen", | ||
| }, | ||
| ) | ||
|
|
||
| # Load controllers | ||
| load_controllers = [] | ||
| for controller in ["panda_arm_controller", "joint_state_controller"]: | ||
| load_controllers += [ | ||
| ExecuteProcess( | ||
| cmd=["ros2 run controller_manager spawner.py {}".format(controller)], | ||
| shell=True, | ||
| output="screen", | ||
| ) | ||
| ] | ||
|
|
||
| # Warehouse mongodb server | ||
| mongodb_server_node = Node( | ||
| package="warehouse_ros_mongo", | ||
| executable="mongo_wrapper_ros.py", | ||
| parameters=[ | ||
| {"warehouse_port": 33829}, | ||
| {"warehouse_host": "localhost"}, | ||
| {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"}, | ||
| ], | ||
| output="screen", | ||
| ) | ||
|
|
||
| return LaunchDescription( | ||
| [ | ||
| rviz_node, | ||
| static_tf, | ||
| robot_state_publisher, | ||
| run_move_group_node, | ||
| ros2_control_node, | ||
| mongodb_server_node, | ||
| ] | ||
| + load_controllers | ||
| ) |
Oops, something went wrong.