diff --git a/README.md b/README.md index ab43326..5b29a47 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ + # cps_loki_bringup This repo houses ROS2 bringup package for CPS Loki robot (small green one). diff --git a/config/box_filter.yaml b/config/box_filter.yaml new file mode 100644 index 0000000..28f59bf --- /dev/null +++ b/config/box_filter.yaml @@ -0,0 +1,15 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: box_filter + type: laser_filters/LaserScanBoxFilter + params: + box_frame: laser_frame + max_x: 0.80 #was 0.16 + max_y: 0.18 #was 0.17 + max_z: 0.1 + min_x: -0.41 #was -0.41 + min_y: -0.18 # was -0.17 + min_z: -0.2 + + invert: false # activate to remove all points outside of the box \ No newline at end of file diff --git a/config/diffbot_controllers.yaml b/config/diffbot_controllers.yaml index 842acf8..e667752 100644 --- a/config/diffbot_controllers.yaml +++ b/config/diffbot_controllers.yaml @@ -29,15 +29,19 @@ diffbot_base_controller: base_frame_id: base_link # pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] # twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - pose_covariance_diagonal: - [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - twist_covariance_diagonal: - [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml + # pose_covariance_diagonal: + # [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + # twist_covariance_diagonal: + # [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml + # Updated covariance matrix to small values on the diagonal axis for package ros_localization. This is recommended for diffdrive rorbots. + pose_covariance_diagonal : [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001] + twist_covariance_diagonal: [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001] + open_loop: false - enable_odom_tf: true + enable_odom_tf: true #disabled because tf will be handled by robot_localizaion node in the future (imu sensor fusion) - cmd_vel_timeout: 0.5 + cmd_vel_timeout: 10.0 # was 0.1 #publish_limited_velocity: true use_stamped_vel: false velocity_rolling_window_size: 10 # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml diff --git a/config/ekf.yaml b/config/ekf.yaml new file mode 100644 index 0000000..c88dbb6 --- /dev/null +++ b/config/ekf.yaml @@ -0,0 +1,60 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of theinputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: true + +# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified. + publish_tf: true #was true + +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" +# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark +# observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node +# from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom # Defaults to "odom" if unspecified + base_link_frame: base_link # Defaults to "base_link" ifunspecified + world_frame: odom # Defaults to the value ofodom_frame if unspecified + + odom0: /odom + # The order of the values of this parameter is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. + # Configure it for x,y,z,vyaw on the odom topic + odom0_config: [true , true , false, + false, false, true, + false, false, false, + false, false, true, + false, false, false] + odom0_differential: true + odom0_relative: true + + imu0: /imu/data_raw + # The order of the values of this parameter is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. + # Configure it for ax,ay,az linear acceleration data for now. Shall not contain values already derived by each other. + # Example: Since angular velocity is fused internally to the IMU to provide the roll, pitch and yaw estimates, we should not fuse in the angular velocities used to derive that information. We also do not fuse in angular velocity due to the noisy characteristics it has when not using exceptionally high quality (and expensive) IMUs. + imu0_config: [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: true + imu0_relative: true + + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set + # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + imu0_remove_gravitational_acceleration: true \ No newline at end of file diff --git a/config/joystick.yaml b/config/joystick.yaml new file mode 100644 index 0000000..819543c --- /dev/null +++ b/config/joystick.yaml @@ -0,0 +1,28 @@ +joy_node: + ros__parameters: + device_id: 0 + deadzone: 0.05 + autorepeat_rate: 0.00 + #coalesce_interval: 0.001 # merging messeges in this interval + +teleop_node: + ros__parameters: + axis_linear: + x: 1 + scale_linear: + x: 0.7 + scale_linear_turbo: + x: 1.5 + + axis_angular: + yaw: 0 + scale_angular: + yaw: 1.4 + scale_angular_turbo: + yaw: 3.0 + + + enable_button: 2 + enable_turbo_button: 5 + + require_enable_button: false diff --git a/config/lsx10.yaml b/config/lsx10.yaml new file mode 100644 index 0000000..d1197eb --- /dev/null +++ b/config/lsx10.yaml @@ -0,0 +1,25 @@ +/lslidar_driver_node: + ros__parameters: + frame_id: laser_frame #激光坐标 + group_ip: 224.1.1.2 + add_multicast: false + device_ip: 192.168.1.200 #雷达源IP + device_ip_difop: 192.168.1.102 #雷达目的ip + msop_port: 2368 #雷达目的端口号 + difop_port: 2369 #雷达源端口号 + lidar_name: N10 #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P + angle_disable_min: 0.0 #角度裁剪开始值 + angle_disable_max: 0.0 #角度裁剪结束值 + min_range: 0.0 #雷达接收距离最小值 + max_range: 200.0 #雷达接收距离最大值 + use_gps_ts: false #雷达是否使用GPS授时 + scan_topic: /scan #设置激光数据topic名称 + interface_selection: serial #接口选择:net 为网口,serial 为串口。 + serial_port_: /dev/ttyUSB0 #串口连接时的串口号 + high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 + compensation: false #M10系列是否使用角度补偿功能 + pubScan: true #是否发布scan话题 + pubPointCloud2: false #是否发布pointcloud2话题 + pointcloud_topic: /lslidar_point_cloud #设置激光数据topic名称 +# pcap: /home/ls/1.pcap #雷达是否使用pcap包读取功能 +# in_file_name: /home/ls/1.txt #雷达是否使用txt文件读取功能 diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index 732a434..ceea797 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -13,8 +13,8 @@ slam_toolbox: odom_frame: odom map_frame: map base_frame: base_footprint - scan_topic: /scan - mode: localization + scan_topic: scan_filtered + mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 7e9886d..7eff0ca 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -13,8 +13,8 @@ slam_toolbox: odom_frame: odom map_frame: map base_frame: base_footprint - scan_topic: /scan - #scan_topic: /scan_filtered + #scan_topic: scan + scan_topic: scan_filtered use_map_saver: true mode: mapping #localization @@ -113,7 +113,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 - scan_topic: scan + scan_topic: scan_filtered bt_navigator: ros__parameters: @@ -183,7 +183,7 @@ bt_navigator_navigate_through_poses_rclcpp_node: bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True - + controller_server: ros__parameters: use_sim_time: True @@ -230,7 +230,7 @@ controller_server: max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 use_interpolation: true # was false - + local_costmap: local_costmap: ros__parameters: @@ -261,7 +261,7 @@ local_costmap: mark_threshold: 0 observation_sources: lslidar lslidar: - topic: scan + topic: scan_filtered max_obstacle_height: 2.0 clearing: True marking: True @@ -284,7 +284,7 @@ global_costmap: robot_base_frame: base_link use_sim_time: True #robot_radius: 0.22 - footprint: "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon + footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] @@ -293,7 +293,7 @@ global_costmap: enabled: True observation_sources: lslidar lslidar: - topic: scan + topic: scan_filtered max_obstacle_height: 2.0 clearing: True marking: True @@ -411,4 +411,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file + velocity_timeout: 1.0 diff --git a/config/twist_mux.yaml b/config/twist_mux.yaml index 375a961..e3333b0 100644 --- a/config/twist_mux.yaml +++ b/config/twist_mux.yaml @@ -1,6 +1,10 @@ twist_mux: ros__parameters: topics: + common: + topic : cmd_vel_common + timeout : 0.5 + priority: 0 navigation: topic : cmd_vel timeout : 0.5 @@ -12,4 +16,4 @@ twist_mux: joystick: topic : cmd_vel_joy timeout : 0.5 - priority: 5 \ No newline at end of file + priority: 50 diff --git a/launch/robot_controller.launch.py b/launch/robot_controller.launch.py index e1a337f..380a06f 100644 --- a/launch/robot_controller.launch.py +++ b/launch/robot_controller.launch.py @@ -72,39 +72,9 @@ def generate_launch_description(): arguments=["diffbot_base_controller", "-c", "/controller_manager"], ) - joystick_spawner = Node( - package="joy", - executable="joy_node" - ) - - teleop_spawner = Node( - package="rmp220_teleop", - executable="rmp220_teleop" - ) - - cam_node = Node( - package="ros2_cam_openCV", - executable="cam_node" - ) - - lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') - lidar_node = LifecycleNode( - package='lslidar_driver', - executable='lslidar_driver_node', - name='lslidar_driver_node', - output='screen', - emulate_tty=True, - namespace='', - parameters=[lidar_dir], - ) - return LaunchDescription([ control_node, robot_state_pub_node, - joint_state_broadcaster_spawner, #commented out because not needed? - robot_controller_spawner, - #joystick_spawner, - #teleop_spawner, - #cam_node, - #lidar_node + joint_state_broadcaster_spawner, + robot_controller_spawner ]) \ No newline at end of file diff --git a/launch/robot_joy_teleop.launch.py b/launch/robot_joy_teleop.launch.py index 5d2e475..794a3ac 100644 --- a/launch/robot_joy_teleop.launch.py +++ b/launch/robot_joy_teleop.launch.py @@ -14,7 +14,7 @@ import os from launch import LaunchDescription -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory @@ -23,6 +23,9 @@ from launch_ros.descriptions import ParameterValue def generate_launch_description(): + #use_sim_time = LaunchConfiguration('use_sim_time') + use_sim_time = True + robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -80,8 +83,7 @@ def generate_launch_description(): teleop_spawner = Node( package="rmp220_teleop", executable="rmp220_teleop", - #remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] - remappings=[('/cmd_vel','/diffbot_base_controller/cmd_vel_unstamped')] + remappings=[('cmd_vel', 'cmd_vel_joy')], ) cam_node = Node( @@ -108,14 +110,33 @@ def generate_launch_description(): remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')] ) + joy_params = os.path.join(get_package_share_directory('cps_loki_bringup'),'config','joystick.yaml') + joy_node = Node( + package='joy', + executable='joy_node', + parameters=[joy_params, {'use_sim_time': use_sim_time}], + #namespace = namespace + ) + + teleop_node = Node( + package='teleop_twist_joy', + executable='teleop_node', + name='teleop_node', + parameters=[joy_params, {'use_sim_time': use_sim_time}], + remappings=[('cmd_vel', 'cmd_vel_joy')], + #namespace = namespace + ) + return LaunchDescription([ #control_node, #robot_state_pub_node, #joint_state_broadcaster_spawner, #robot_controller_spawner, - joystick_spawner, - teleop_spawner, + #joystick_spawner, + #teleop_spawner, #cam_node, #lidar_node, - #twist_mux + twist_mux, + joy_node, + teleop_node ]) diff --git a/launch/robot_lidar.launch.py b/launch/robot_lidar.launch.py index cf4af81..92b3a08 100644 --- a/launch/robot_lidar.launch.py +++ b/launch/robot_lidar.launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): executable="cam_node" ) - lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_dir = os.path.join(get_package_share_directory('cps_loki_bringup'), 'config', 'lsx10.yaml') lidar_node = LifecycleNode( package='lslidar_driver', executable='lslidar_driver_node', @@ -107,4 +107,4 @@ def generate_launch_description(): #teleop_spawner, #cam_node, lidar_node - ]) \ No newline at end of file + ]) diff --git a/launch/robot_localization.launch.py b/launch/robot_localization.launch.py index e969f6a..97ca652 100644 --- a/launch/robot_localization.launch.py +++ b/launch/robot_localization.launch.py @@ -118,6 +118,22 @@ def generate_launch_description(): parameters=[lidar_dir], ) + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters = [ PathJoinSubstitution( + [ + FindPackageShare("cps_loki_bringup"), + "config", + "ekf.yaml" + ] + ) , + #{'use_sim_time': LaunchConfiguration('use_sim_time')} + ] + ) + return LaunchDescription([ #control_node, #robot_state_pub_node, @@ -127,5 +143,6 @@ def generate_launch_description(): #teleop_spawner, #cam_node, #lidar_node, - mapper_node + #mapper_node, + robot_localization_node ]) \ No newline at end of file diff --git a/launch/robot_scan_filter.launch.py b/launch/robot_scan_filter.launch.py new file mode 100644 index 0000000..41dd487 --- /dev/null +++ b/launch/robot_scan_filter.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[ + PathJoinSubstitution([ + get_package_share_directory("cps_loki_bringup"), + "config", "box_filter.yaml", + ])], + ) + ]) \ No newline at end of file diff --git a/launch/rsp.launch.py b/launch/rsp.launch.py index 0463cd8..83660c7 100644 --- a/launch/rsp.launch.py +++ b/launch/rsp.launch.py @@ -38,21 +38,6 @@ def generate_launch_description(): ) robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("cps_loki_bringup"), - "config", - "diffbot_controllers.yaml", - ] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - output="both", - ) - robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -60,51 +45,6 @@ def generate_launch_description(): parameters=[robot_description], ) - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["diffbot_base_controller", "-c", "/controller_manager"], - ) - - joystick_spawner = Node( - package="joy", - executable="joy_node" - ) - - teleop_spawner = Node( - package="rmp220_teleop", - executable="rmp220_teleop" - ) - - cam_node = Node( - package="ros2_cam_openCV", - executable="cam_node" - ) - - lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') - lidar_node = LifecycleNode( - package='lslidar_driver', - executable='lslidar_driver_node', - name='lslidar_driver_node', - output='screen', - emulate_tty=True, - namespace='', - parameters=[lidar_dir], - ) - return LaunchDescription([ - #control_node, - robot_state_pub_node, - #joint_state_broadcaster_spawner, - #robot_controller_spawner, - #joystick_spawner, - #teleop_spawner, - #cam_node, - #lidar_node + robot_state_pub_node ]) \ No newline at end of file diff --git a/package.xml b/package.xml index b48721d..639aa66 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ cps_loki_bringup 0.0.2 - ROS2 bringup package for CPS Loki Robot. + ROS2 bringup sources for CPS Loki Robot bjorn TODO: License declaration @@ -18,9 +18,11 @@ navigation2 ros2_control ros2_controllers - ros2_cam_openCV + teleop_twist_joy joy twist_mux + robot_localization + laser_filters ament_python