diff --git a/rebuild.sh b/rebuild.sh new file mode 100755 index 0000000..1f2c0a6 --- /dev/null +++ b/rebuild.sh @@ -0,0 +1,7 @@ +#!/bin/bash +cd src +rm -r install build log +colcon build --packages-select robot_interfaces robot_controller +source install/local_setup.bash +colcon build +source install/local_setup.bash diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index 778e411..4b7a634 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -47,11 +47,12 @@ def launch_setup(context, *args, **kwargs): ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) - ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem' - controllers_name = 'fake_controllers' - moveit_controller_manager_key = 'moveit_simple_controller_manager' - moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') + moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager') + moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager') # robot moveit common launch # xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -214,7 +215,6 @@ def launch_setup(context, *args, **kwargs): } - nodes = [ Node( package="lite6_controller", @@ -236,11 +236,238 @@ def launch_setup(context, *args, **kwargs): ), ] +# ######################3 + + #moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_fake_controller_manager') + #moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_fake_controller_manager/MoveItFakeControllerManager') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + + moveit_config_package_name = 'xarm_moveit_config' + xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) + + # robot_description_parameters + # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) + get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters') + robot_description_parameters = get_xarm_robot_description_parameters( + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), + urdf_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i': add_realsense_d435i, + 'add_other_geometry': add_other_geometry, + 'geometry_type': geometry_type, + 'geometry_mass': geometry_mass, + 'geometry_height': geometry_height, + 'geometry_radius': geometry_radius, + 'geometry_length': geometry_length, + 'geometry_width': geometry_width, + 'geometry_mesh_filename': geometry_mesh_filename, + 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, + 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, + 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, + 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, + }, + srdf_arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_other_geometry': add_other_geometry, + }, + arguments={ + 'context': context, + 'xarm_type': xarm_type, + } + ) + + load_yaml = getattr(mod, 'load_yaml') + controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context))) + ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + kinematics_yaml = robot_description_parameters['robot_description_kinematics'] + joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) + + if add_gripper.perform(context) in ('True', 'true'): + gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context))) + gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml') + gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml') + + if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml: + for name in gripper_controllers_yaml['controller_names']: + if name in gripper_controllers_yaml: + if name not in controllers_yaml['controller_names']: + controllers_yaml['controller_names'].append(name) + controllers_yaml[name] = gripper_controllers_yaml[name] + if gripper_ompl_planning_yaml: + ompl_planning_yaml.update(gripper_ompl_planning_yaml) + if joint_limits_yaml and gripper_joint_limits_yaml: + joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits']) + + add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') + add_prefix_to_moveit_params( + controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml, + kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml, + prefix=prefix.perform(context)) + + + + # Planning pipeline + # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py + planning_pipeline = { + "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], + "default_planning_pipeline": "pilz_industrial_motion_planner", + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved + "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", + # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled + "start_state_max_bounds_error": 0.31416, + }, + "pilz_industrial_motion_planner": { + "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner", + "default_planner_config": "PTP", + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService", + }, + } + # Kinematics + #kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml') + + + + + + # Planning Configuration + 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_pipeline_config['move_group'].update(ompl_planning_yaml) + + pilz_planning_pipeline_config = { + 'move_group': { + 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', + '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""", + "default_planner_config": "PTP", + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService", + } + } + + + # Moveit controllers Configuration + moveit_controllers = { + moveit_controller_manager_key.perform(context): controllers_yaml, + 'moveit_controller_manager': moveit_controller_manager_value.perform(context), + } + + # Trajectory Execution Configuration + 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, + 'trajectory_execution.execution_duration_monitoring': False + } + + plan_execution = { + 'plan_execution.record_trajectory_state_frequency': 10.0, + } + + planning_scene_monitor_parameters = { + 'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True, + # "planning_scene_monitor_options": { + # "name": "planning_scene_monitor", + # "robot_description": "robot_description", + # "joint_state_topic": "/joint_states", + # "attached_collision_object_topic": "/move_group/planning_scene_monitor", + # "publish_planning_scene_topic": "/move_group/publish_planning_scene", + # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", + # "wait_for_initial_state_timeout": 10.0, + # }, + } + + # Start the actual move_group node/action server + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[ + robot_description_parameters, + #ompl_planning_pipeline_config, + pilz_planning_pipeline_config, + trajectory_execution, + plan_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': use_sim_time}, + ], + ) + + # rviz with moveit configuration + # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file], + parameters=[ + robot_description_parameters, + ompl_planning_pipeline_config, + {'use_sim_time': use_sim_time}, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # Static TF + static_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'], + parameters=[{'use_sim_time': use_sim_time}], + ) + return [ - robot_moveit_common_launch, + #RegisterEventHandler(event_handler=OnProcessExit( + # target_action=rviz2_node, + # on_exit=[EmitEvent(event=Shutdown())] + #)), + rviz2_node, + static_tf, + move_group_node, robot_gazebo_launch, ] + nodes +# ######################3 + + #return [ + # robot_moveit_common_launch, + # robot_gazebo_launch, + #] + nodes + def generate_launch_description(): return LaunchDescription([