diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt index 1395c06..61c1f73 100644 --- a/src/lite6_controller/CMakeLists.txt +++ b/src/lite6_controller/CMakeLists.txt @@ -7,9 +7,19 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(robot_interfaces REQUIRED) +find_package(robot_controller REQUIRED) +find_package(moveit REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(Eigen3 REQUIRED) +include_directories(include ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -22,16 +32,24 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -find_package(robot_controller REQUIRED) -find_package(rclcpp REQUIRED) +add_executable(lite6_controller src/lite6_controller.cpp) +ament_target_dependencies(lite6_controller + "rclcpp" + "rclcpp_action" + "Eigen3" + "robot_controller" + "moveit_ros_planning_interface" + "robot_interfaces") -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Motion.msg" - "srv/ExecuteMotion.srv" - DEPENDENCIES geometry_msgs # Add packages that above messages depend on -) +install(TARGETS + lite6_controller + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py new file mode 100644 index 0000000..778e411 --- /dev/null +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -0,0 +1,248 @@ +#!/usr/bin/env -S ros2 launch +"""Launch Python example for following a target""" + +import os +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + use_sim_time = LaunchConfiguration("use_sim_time", default=True) + log_level = LaunchConfiguration("log_level", default='warn') + rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz")) + + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=False) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='lite') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + #geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type = LaunchConfiguration('geometry_type', default='cylinder') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.05) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.005) + geometry_length = LaunchConfiguration('geometry_length', default=0.07) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + load_controller = LaunchConfiguration('load_controller', default=True) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + + 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' + + # robot moveit common launch + # xarm_moveit_config/launch/_robot_moveit_common.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'no_gui_ctrl': 'false', + 'ros2_control_plugin': ros2_control_plugin, + 'controllers_name': controllers_name, + 'moveit_controller_manager_key': moveit_controller_manager_key, + 'moveit_controller_manager_value': moveit_controller_manager_value, + '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, + 'use_sim_time': 'true' + }.items(), + ) + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazebo_launch = IncludeLaunchDescription( + #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])), + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + '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, + 'load_controller': 'true', + }.items(), + ) + + # URDF + _robot_description_xml = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro'] + ), + #PathJoinSubstitution( + # [ + # FindPackageShare('xarm_description'), + # "urdf", + # "lite6", + # #"lite6.urdf.xacro", + # "lite6_robot_macro.xacro", + # ] + #), + " ", + #"name:=", "lite6", " ", + "prefix:=", " ", + "hw_ns:=", hw_ns, " ", + "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, " ", + #"ros2_control_params:=", ros2_control_params, " ", + + "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, " ", + + #"robot_ip:=", robot_ip, " ", + #"report_type:=", report_type, " ", + #"baud_checkset:=", baud_checkset, " ", + #"default_gripper_baud:=", default_gripper_baud, " ", + ] + ) + # TODO fix URDF loading + # xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0" + _robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])]) + robot_description = {"robot_description": _robot_description_xml} + # SRDF + _robot_description_semantic_xml = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare('xarm_moveit_config'), + "srdf", + #"_lite6_macro.srdf.xacro", + "xarm.srdf.xacro", + ] + ), + " ", + #"name:=", "lite6", " ", + "prefix:=", " ", + #"hw_ns:=", hw_ns, " ", + #"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, " ", + #"ros2_control_params:=", ros2_control_params, " ", + + #"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, " ", + + #"robot_ip:=", robot_ip, " ", + #"report_type:=", report_type, " ", + #"baud_checkset:=", baud_checkset, " ", + #"default_gripper_baud:=", default_gripper_baud, " ", + ] + ) + robot_description_semantic = { + "robot_description_semantic": _robot_description_semantic_xml + } + + + + nodes = [ + Node( + package="lite6_controller", + executable="lite6_controller", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + robot_description, + robot_description_semantic, + {"use_sim_time": use_sim_time}, + ], + ), + Node( + package="draw_svg", + executable="drawing_surface.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return [ + robot_moveit_common_launch, + robot_gazebo_launch, + ] + nodes + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py new file mode 100644 index 0000000..b80b288 --- /dev/null +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -0,0 +1,361 @@ +import os +from launch import LaunchDescription +from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown + + +def launch_setup(context, *args, **kwargs): + robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150') + report_type = LaunchConfiguration('report_type', default='normal') + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='lite') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + + baud_checkset = LaunchConfiguration('baud_checkset', default=True) + default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000) + + ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware' + controllers_name = LaunchConfiguration('controllers_name', default='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') + + xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + log_level = LaunchConfiguration("log_level", default='warn') + + # # robot driver launch + # # xarm_api/launch/_robot_driver.launch.py + # robot_driver_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])), + # launch_arguments={ + # 'robot_ip': robot_ip, + # 'report_type': report_type, + # 'dof': dof, + # 'hw_ns': hw_ns, + # 'add_gripper': add_gripper, + # 'prefix': prefix, + # 'baud_checkset': baud_checkset, + # 'default_gripper_baud': default_gripper_baud, + # 'robot_type': robot_type, + # }.items(), + # ) + + # robot description launch + # xarm_description/launch/_robot_description.launch.py + robot_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + '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, + 'joint_states_remapping': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']), + '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, + }.items(), + ) + + # robot_description_parameters + # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py + moveit_config_package_name = 'xarm_moveit_config' + 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_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 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) + + # 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, + 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'], + ) + + + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}], + remappings=[ + ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)), + ], + ) + + # ros2 control launch + # xarm_controller/launch/_ros2_control.launch.py + ros2_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'baud_checkset': baud_checkset, + 'default_gripper_baud': default_gripper_baud, + 'prefix': prefix, + 'hw_ns': hw_ns, + '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, + }.items(), + ) + + control_node = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + ) + + nodes = [ + Node( + package="draw_svg", + executable="follow", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + #robot_description_parameters['robot_description'], + #robot_description_parameters['robot_description_semantic'], + #robot_description_parameters['robot_description_planning'], + #robot_description_parameters['robot_description_kinematics'], + robot_description_parameters, + {"use_sim_time": use_sim_time}, + ], + ), + Node( + package="draw_svg", + executable="draw_svg.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return [ + + RegisterEventHandler(event_handler=OnProcessExit( + target_action=rviz2_node, + on_exit=[EmitEvent(event=Shutdown())] + )), + rviz2_node, + static_tf, + move_group_node, + + + robot_description_launch, + joint_state_publisher_node, + ros2_control_launch, + control_node, + # robot_driver_launch, + ] + nodes + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/lite6_controller/package.xml b/src/lite6_controller/package.xml index 1db4938..e5d246e 100644 --- a/src/lite6_controller/package.xml +++ b/src/lite6_controller/package.xml @@ -9,6 +9,22 @@ ament_cmake + rclcpp + + + moveit + moveit_msgs + geometry_msgs + moveit_ros_planning_interface + + eigen + geometric_shapes + moveit_common + moveit_ros_planning + + xarm_description + xarm_moveit_config + ament_lint_auto ament_lint_common diff --git a/src/lite6_controller/rebuild.sh b/src/lite6_controller/rebuild.sh deleted file mode 100644 index 167eb5f..0000000 --- a/src/lite6_controller/rebuild.sh +++ /dev/null @@ -1,8 +0,0 @@ -rm -r build/ install/ log/ -rosdep install --from-paths . --ignore-src -r -y -colcon build -vcs import --recursive --shallow . < lite6_controller.repos -rosdep update -apt-get update -rosdep install -y -r -i -#ros2 launch draw_svg draw_svg.launch.py diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp new file mode 100644 index 0000000..766b651 --- /dev/null +++ b/src/lite6_controller/src/lite6_controller.cpp @@ -0,0 +1,112 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +const std::string MOVE_GROUP = "lite6"; + +// +class Lite6Controller : public RobotController +{ +public: + /// Move group interface for the robot + moveit::planning_interface::MoveGroupInterface move_group; + + //bool moved = false; + + Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : RobotController(options), + move_group(std::shared_ptr(std::move(this)), MOVE_GROUP) + { + // Use upper joint velocity and acceleration limits + //this->move_group.setMaxAccelerationScalingFactor(1.0); + //this->move_group.setMaxVelocityScalingFactor(1.0); + + // Subscribe to target pose + //target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "Initialization successful."); + + } + + // TODO implement time param + // https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html + // https://groups.google.com/g/moveit-users/c/MOoFxy2exT4 + // https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotTrajectory.html + + // TODO implement feedback + // https://answers.ros.org/question/249995/how-to-check-sate-of-plan-execution-in-moveit-during-async-execution-in-python/ + + /// Callback that executes path on robot + virtual void executePath(const std::shared_ptr> goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(20); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + std::vector waypoints; + + for (auto p : goal->motion.path) + waypoints.push_back(p.pose); + + moveit_msgs::msg::RobotTrajectory trajectory; + + // dangerous with real robot + // https://moveit.picknik.ai/galactic/doc/examples/move_group_interface/move_group_interface_tutorial.html + const double jump_threshold = 0.0; + + const double eef_step = 0.01; + double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); + + this->move_group.execute(trajectory); + + //waypoints.clear(); + + //for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) { + // // Check if there is a cancel request + // if (goal_handle->is_canceling()) { + // result->result = feedback->status; + // goal_handle->canceled(result); + // RCLCPP_INFO(this->get_logger(), "Goal canceled"); + // return; + // } + // // Update status + // feedback->status = std::to_string(i) + "/10 complete"; + // // Publish feedback + // goal_handle->publish_feedback(feedback); + // RCLCPP_INFO(this->get_logger(), feedback->status.c_str()); + + // loop_rate.sleep(); + //} + + // Check if goal is done + if (rclcpp::ok()) { + result->result = "success"; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting lite6_controller"); + auto lite6 = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(lite6); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/src/robot_controller/CMakeLists.txt b/src/robot_controller/CMakeLists.txt index 1a9d932..312b54c 100644 --- a/src/robot_controller/CMakeLists.txt +++ b/src/robot_controller/CMakeLists.txt @@ -15,19 +15,43 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(robot_interfaces REQUIRED) -add_executable(robot_controller src/cpp/robot_controller.cpp) +include_directories(include) + +add_library(robot_controller src/cpp/robot_controller.cpp) +add_executable(dummy_controller src/cpp/dummy_controller.cpp) ament_target_dependencies(robot_controller "rclcpp" "rclcpp_action" "robot_interfaces") +ament_target_dependencies(dummy_controller + "rclcpp" + "rclcpp_action" + "robot_interfaces") -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +ament_export_targets(robot_controller HAS_LIBRARY_TARGET) -install(TARGETS - robot_controller +install( + DIRECTORY include/robot_controller + DESTINATION include/ +) +install( + TARGETS robot_controller + EXPORT robot_controller + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +#if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +#endif() + +install( + TARGETS dummy_controller DESTINATION lib/${PROJECT_NAME}) + + ament_package() diff --git a/src/robot_controller/include/robot_controller/robot_controller.hpp b/src/robot_controller/include/robot_controller/robot_controller.hpp new file mode 100644 index 0000000..c89c870 --- /dev/null +++ b/src/robot_controller/include/robot_controller/robot_controller.hpp @@ -0,0 +1,85 @@ +#ifndef ROBOT_CONTROLLER_H +#define ROBOT_CONTROLLER_H + +#include "robot_interfaces/action/execute_motion.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +class RobotController : public rclcpp::Node +{ +public: + using ExecuteMotion = robot_interfaces::action::ExecuteMotion; + using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle; + + explicit RobotController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + rclcpp_action::Server::SharedPtr action_server_; + + virtual rclcpp_action::GoalResponse motion_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + virtual rclcpp_action::CancelResponse motion_handle_cancel( + const std::shared_ptr goal_handle); + + virtual void motion_handle_accepted(const std::shared_ptr goal_handle); + + /// Callback that executes path on robot + virtual void executePath(const std::shared_ptr goal_handle); +}; + +using ExecuteMotion = robot_interfaces::action::ExecuteMotion; +using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle; + +RobotController::RobotController(const rclcpp::NodeOptions & options) +: Node("robot_controller",options) +{ + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server( + this, + "execute_motion", + std::bind(&RobotController::motion_handle_goal, this, _1, _2), + std::bind(&RobotController::motion_handle_cancel, this, _1), + std::bind(&RobotController::motion_handle_accepted, this, _1)); +} + +rclcpp_action::GoalResponse RobotController::motion_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received goal request with acceleration %f", goal->motion.acceleration); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse RobotController::motion_handle_cancel( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void RobotController::motion_handle_accepted(const std::shared_ptr goal_handle) +{ + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach(); +} + +/// Callback that executes path on robot +void RobotController::executePath(const std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + std::string msg = "executePath not implemented"; + result->result = msg; + feedback->status = msg; + RCLCPP_WARN(this->get_logger(), msg.c_str()); +} + +#endif diff --git a/src/robot_controller/src/cpp/dummy_controller.cpp b/src/robot_controller/src/cpp/dummy_controller.cpp new file mode 100644 index 0000000..ffc51f4 --- /dev/null +++ b/src/robot_controller/src/cpp/dummy_controller.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include + +#include "robot_controller/robot_controller.hpp" +#include "robot_interfaces/action/execute_motion.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation. +class DummyController : public RobotController +{ + public: + DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {} + + /// Callback that executes path on robot + virtual void executePath(const std::shared_ptr> goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(20); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + + for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) { + // Check if there is a cancel request + if (goal_handle->is_canceling()) { + result->result = feedback->status; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + // Update status + feedback->status = std::to_string(i) + "/10 complete"; + // Publish feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->status.c_str()); + + loop_rate.sleep(); + } + + // Check if goal is done + if (rclcpp::ok()) { + result->result = "success"; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller"); + auto robot = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(robot); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/src/robot_controller/src/cpp/robot_controller.cpp b/src/robot_controller/src/cpp/robot_controller.cpp index 6e0f552..cb5fcb1 100644 --- a/src/robot_controller/src/cpp/robot_controller.cpp +++ b/src/robot_controller/src/cpp/robot_controller.cpp @@ -3,6 +3,7 @@ #include #include +#include "robot_controller/robot_controller.hpp" #include "robot_interfaces/action/execute_motion.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -12,118 +13,3 @@ // // https://docs.ros.org/en/foxy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html -class RobotController : public rclcpp::Node -{ -public: - using ExecuteMotion = robot_interfaces::action::ExecuteMotion; - using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle; - - explicit RobotController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("robot_controller",options) - { - using namespace std::placeholders; - - this->action_server_ = rclcpp_action::create_server( - this, - "execute_motion", - std::bind(&RobotController::motion_handle_goal, this, _1, _2), - std::bind(&RobotController::motion_handle_cancel, this, _1), - std::bind(&RobotController::motion_handle_accepted, this, _1)); - } - -private: - rclcpp_action::Server::SharedPtr action_server_; - - virtual rclcpp_action::GoalResponse motion_handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request with acceleration %f", goal->motion.acceleration); - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - virtual rclcpp_action::CancelResponse motion_handle_cancel( - const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - virtual void motion_handle_accepted(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach(); - } - - /// Callback that executes path on robot - virtual void executePath(const std::shared_ptr goal_handle) - { - const auto goal = goal_handle->get_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); - std::string msg = "executePath not implemented"; - result->result = msg; - feedback->status = msg; - RCLCPP_WARN(this->get_logger(), msg.c_str()); - } -}; - -// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation. -class DummyController : public RobotController -{ - public: - DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {} - - /// Callback that executes path on robot - virtual void executePath(const std::shared_ptr> goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing goal"); - rclcpp::Rate loop_rate(20); - const auto goal = goal_handle->get_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - - for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) { - // Check if there is a cancel request - if (goal_handle->is_canceling()) { - result->result = feedback->status; - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Goal canceled"); - return; - } - // Update status - feedback->status = std::to_string(i) + "/10 complete"; - // Publish feedback - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->status.c_str()); - - loop_rate.sleep(); - } - - // Check if goal is done - if (rclcpp::ok()) { - result->result = "success"; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal succeeded"); - } - } -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller"); - auto robot = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(robot); - executor.spin(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -}