diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index f969f2a..66effbc 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -3,7 +3,7 @@ run-name: ${{ github.actor }} is working on ROS2 rove build on: [push] jobs: Build_rove: - runs-on: [self-hosted, linux, docker] + runs-on: ubuntu-latest permissions: packages: write contents: read diff --git a/src/rove_bringup/config/test.xml b/src/rove_bringup/config/test.xml new file mode 100644 index 0000000..e75ca66 --- /dev/null +++ b/src/rove_bringup/config/test.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/rove_navigation/config/twist_mux.yaml b/src/rove_bringup/config/twist_mux.yaml similarity index 53% rename from src/rove_navigation/config/twist_mux.yaml rename to src/rove_bringup/config/twist_mux.yaml index e960825..893dd7c 100644 --- a/src/rove_navigation/config/twist_mux.yaml +++ b/src/rove_bringup/config/twist_mux.yaml @@ -1,11 +1,15 @@ -twist_mux: - ros_parameters: +/twist_mux: + ros__parameters: topics: navigation: - topic: cmd_vel + topic: nav_vel timeout: 0.5 priority: 10 joystick: - topic: cmd_vel_joy + topic: joy_vel timeout: 0.5 priority: 20 + others: + topic: cmd_vel + timeout: 0.5 + priority: 1 \ No newline at end of file diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index 076b9d1..650994f 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): os.path.join(slam_pkg_path, "launch", "online_async_launch.py"), ), launch_arguments={ - "use_sim_time": "true", + "use_sim_time": use_sim_time, "slam_params_file": os.path.join( pkg_rove_slam, "config", "slam_config.yaml" ) @@ -34,7 +34,7 @@ def generate_launch_description(): slam3d = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"), + os.path.join(pkg_rove_slam, "launch", "velodyne_3d.launch.py"), ), launch_arguments={ "use_sim_time": use_sim_time, diff --git a/src/rove_bringup/launch/behavior.launch.py b/src/rove_bringup/launch/behavior.launch.py new file mode 100644 index 0000000..2223e41 --- /dev/null +++ b/src/rove_bringup/launch/behavior.launch.py @@ -0,0 +1,24 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions.node import Node + +def generate_launch_description(): + # Path + bt_xml_dir = os.path.join(get_package_share_directory('rove_bringup'), 'config') + + # Parameters + bt_xml = LaunchConfiguration('bt_xml', default=bt_xml_dir+'/test.xml') + + behavior_tree = Node( + package='bt_ros2', + executable='bt_ros2', + parameters=[{'bt_xml': bt_xml}], + output='screen' + ) + + ld = LaunchDescription() + ld.add_action(behavior_tree) + return ld \ No newline at end of file diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index ec0935d..500fe90 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -97,12 +97,22 @@ def generate_launch_description(): "use_slam3d": "true", }.items(), ) + + twist_mux = Node( + package='twist_mux', + executable='twist_mux', + output='screen', + parameters=[os.path.join(pkg_rove_bringup, 'config/twist_mux.yaml')], + remappings={ + ('/cmd_vel_out', '/diff_drive_controller/cmd_vel_unstamped') + }) return LaunchDescription([ robot_state_publisher, robot_localization_node_local, - #robot_localization_node_global, + # robot_localization_node_global, #navsat_transform, + twist_mux, rviz, teleop, autonomy, diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index aabd6e5..695418d 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -131,7 +131,7 @@ def create_controller_node(node_name:str): zed = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), - ) + ), ) return LaunchDescription([ diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py index 43dc67e..f63d9ea 100644 --- a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py +++ b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): parameters=[teleop_joy_params_file], remappings=[ ('/joy', '/joy'), - # ('/cmd_vel', '/model/rove/cmd_vel') + ('/cmd_vel', '/joy_vel'), ], ), Node( diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py index 61153ce..993a316 100644 --- a/src/rove_bringup/launch/rove_controller_usb.launch.py +++ b/src/rove_bringup/launch/rove_controller_usb.launch.py @@ -25,6 +25,7 @@ def generate_launch_description(): parameters=[teleop_joy_params_file], remappings=[ ('/joy', '/joy'), + ('/cmd_vel', '/joy_vel'), ], ), Node( diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index b13a321..52eb91e 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/src/rove_bringup/launch/sim.launch.py @@ -8,6 +8,7 @@ from launch.substitutions import Command from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from math import pi def generate_launch_description(): @@ -31,19 +32,48 @@ def generate_launch_description(): launch_arguments={'gz_args': "-v 4 -r " + world}.items(), ) - # Spawn robot - create = Node( + walls_file_path = os.path.join(pkg_rove_description, 'worlds', 'walls.sdf') + spawn_walls = Node( package='ros_gz_sim', executable='create', - arguments=['-name', 'rove', - '-topic', 'robot_description', + arguments=['-file', walls_file_path, + '-name', 'walls', '-x', '0', '-y', '0', - '-z', '0.1', - ], + '-z', '0'], output='screen', ) + actor_file_path = os.path.join(pkg_rove_description, 'worlds', 'actor.sdf') + spawn_actor = Node( + package='ros_gz_sim', + executable='create', + arguments=['-file', actor_file_path, + '-name', 'actor', + '-topic', 'actor_pose', + '-x', '0', + '-y', '0', + '-z', '0.1'], + output='screen', + ) + + yaw = -pi / 2 + + # Spawn robot + spawn_rove = Node( + package='ros_gz_sim', + executable='create', + arguments=[ + '-name', 'rove', + '-topic', 'robot_description', + '-x', '0', + '-y', '0', + '-z', '0.1', + '-Y', str(yaw), + ], + output='screen', +) + # Takes the description and joint angles as inputs and publishes # the 3D poses of the robot links robot_state_publisher = Node( @@ -57,6 +87,14 @@ def generate_launch_description(): ] ) + # fake human tracker + human_tracker = Node( + package='rove_navigation', + executable='green_person_tracker', + name='green_person_tracker', + output='screen', + ) + # Bridge ROS topics and Gazebo messages for establishing communication bridge = Node( package='ros_gz_bridge', @@ -82,7 +120,10 @@ def generate_launch_description(): return LaunchDescription([ gz_sim, bridge, + spawn_walls, + spawn_actor, robot_state_publisher, - create, + spawn_rove, common, + human_tracker, ]) diff --git a/src/rove_bringup/package.xml b/src/rove_bringup/package.xml index fe05692..6a11eab 100644 --- a/src/rove_bringup/package.xml +++ b/src/rove_bringup/package.xml @@ -9,12 +9,14 @@ joy teleop_twist_joy + twist_mux std_msgs geometry_msgs control_msgs joy teleop_twist_joy + twist_mux control_msgs foxglove_bridge diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 60dbd12..3d4e9d3 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /DepthCloud1/Auto Size1 Splitter Ratio: 0.5 - Tree Height: 517 + Tree Height: 325 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -118,6 +118,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + zed_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -179,79 +184,19 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/TF - Enabled: true + Enabled: false Frame Timeout: 15 Frames: All Enabled: false - base_link: - Value: true - camera_link: - Value: true - flipper_fl: - Value: true - flipper_fr: - Value: true - flipper_rl: - Value: true - flipper_rr: - Value: true - map: - Value: true - odom: - Value: true - sensor_link: - Value: true - track_fl: - Value: true - track_fr: - Value: true - track_l: - Value: true - track_r: - Value: true - track_rl: - Value: true - track_rr: - Value: true - velodyne_laser: - Value: true - velodyne_link: - Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - map: - odom: - base_link: - flipper_fl: - {} - flipper_fr: - {} - flipper_rl: - {} - flipper_rr: - {} - sensor_link: - camera_link: - {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: - velodyne_laser: - {} + {} Update Interval: 0 - Value: true + Value: false - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map @@ -357,7 +302,7 @@ Visualization Manager: y_scale: 1 z_scale: 1 Class: rviz_imu_plugin/Imu - Enabled: true + Enabled: false Name: Imu Topic: Depth: 5 @@ -366,7 +311,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /imu - Value: true + Value: false fixed_frame_orientation: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -400,7 +345,7 @@ Visualization Manager: Node filtering radius (m): 0 Position Transformer: XYZ Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -451,6 +396,48 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /zed/zed_node/left/image_rect_color + Value: true + Visibility: + DepthCloud: true + Grid: true + Imu: true + LaserScan: true + Map: true + MapCloud: true + Path: true + PointStamped: true + RobotModel: true + TF: true + Value: true + filtered position: true + Zoom Factor: 1 + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /person_position + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -497,33 +484,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 11.600849151611328 + Distance: 12.128095626831055 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.209733486175537 - Y: -1.5692439079284668 - Z: 2.5361857414245605 + X: -0.19487914443016052 + Y: -0.5717933773994446 + Z: 2.147127151489258 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9553995728492737 + Pitch: 0.6703999042510986 Target Frame: Value: Orbit (rviz) - Yaw: 3.6536216735839844 + Yaw: 2.9836294651031494 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 814 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000afb000000100044006900730070006c006100790073010000003d00000290000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000bfb000000100044006900730070006c006100790073010000003d000001d0000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000fb0000000c00430061006d0065007200610100000213000000ba0000002800ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml index 29566e2..9ab1e6e 100644 --- a/src/rove_description/config/default_bridge.yaml +++ b/src/rove_description/config/default_bridge.yaml @@ -6,7 +6,7 @@ direction: GZ_TO_ROS # gz topic subscribed to by Tracked Vehicle plugin -- ros_topic_name: "cmd_vel" +- ros_topic_name: "diff_drive_controller/cmd_vel_unstamped" gz_topic_name: "cmd_vel" ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro index 45461da..a061319 100644 --- a/src/rove_description/urdf/rove.ros2_control.urdf.xacro +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -1,14 +1,33 @@ - + + + ${nodeid} + + + + + + + + + + + + + + + + + - + - + - + @@ -18,91 +37,22 @@ odrive_ros2_control_plugin/ODriveHardwareInterface can0 - - 23 - - - - - - - 24 - - - - - - - - 21 - - - - - - - 22 - - - - - - - - + + + + - - + + + + + - - - \ No newline at end of file diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro index 2f3ac52..5b3dad7 100644 --- a/src/rove_description/urdf/rove.urdf.xacro +++ b/src/rove_description/urdf/rove.urdf.xacro @@ -42,7 +42,9 @@ - + + + diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index 3b01c59..b250f45 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -8,6 +8,12 @@ + + + + + + @@ -32,6 +38,24 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/rove_description/worlds/actor.sdf b/src/rove_description/worlds/actor.sdf new file mode 100644 index 0000000..886fe60 --- /dev/null +++ b/src/rove_description/worlds/actor.sdf @@ -0,0 +1,85 @@ + + + + + 0 0 0 0 0 0 + + + + 1 1 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + + + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + + + + \ No newline at end of file diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world index af8a24a..e5211ca 100644 --- a/src/rove_description/worlds/base_world.world +++ b/src/rove_description/worlds/base_world.world @@ -116,8 +116,9 @@ - 3.5 0 0 0 0 0 - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree + 3.5 0 0 0 0 3.14 + + https://fuel.gazebosim.org/1.0/sebbyjp/models/Sofa @@ -162,872 +163,5 @@ - - - 0 0 0 0 0 0 - - - - - 6 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 6 0.15 2.5 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - 0 - - - -12.4007 5.45 0 0 -0 1.5708 - - - - - - 1 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 1 0.15 2.5 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - 0 - - - -11.9757 8.375 0 0 -0 0 - - - - - - 4.25 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 4.25 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -7.17535 9.4 0 0 -0 0.523599 - - - - - - 9.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 9.75 0.15 2.5 - - - - 1 0 1 1 - 1 0 1 1 - 1 0 1 1 - - - 0 - - - -0.6 10.425 0 0 -0 0 - - - - - - 6 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 6 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 4.2 7.5 0 0 -0 -1.5708 - - - - - - 12 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 12 0.15 2.5 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - 0 - - - 4.2 -1.35 0 0 -0 -1.5708 - - - - - - 6.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 6.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 7.375 -7.275 0 0 -0 0 - - - - - - 14 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 14 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 10.55 -0.35 0 0 -0 1.5708 - - - - - - 17 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 17 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 18.975 6.575 0 0 -0 0 - - - - - - 19 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 19 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 27.4 -2.85 0 0 -0 -1.5708 - - - - - - 30.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 30.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 12.1 -12.275 0 0 -0 3.14159 - - - - - - 15 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 15 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -3.2 -4.85 0 0 -0 1.5708 - - - - - - 1.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 1.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -10.985 7.80931 0 0 -0 -0.785398 - - - - - - 4.25 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 4.25 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -5.25 2.575 0 0 -0 3.14159 - - - - - - 7.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 7.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -7.3 -1.1 0 0 -0 -1.5708 - - - - - - 11.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 11.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -13.1 -4.775 0 0 -0 3.14159 - - - - - - 4.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 4.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -18.9 -6.95 0 0 -0 -1.5708 - - - - - - 9 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 9 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -14.475 -9.125 0 0 -0 0 - - - - - - 3.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 3.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -10.05 -10.925 0 0 -0 -1.5708 - - - - - - 17.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 17.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -18.725 -12.725 0 0 -0 3.14159 - - - - - - 13.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 13.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -27.4 -5.925 0 0 -0 1.5708 - - - - - - 7.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 7.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -23.6 0.875 0 0 -0 0 - - - - - - 12 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 12 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -19.8 6.8 0 0 -0 1.5708 - - - - - - 1.78502 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 1.78502 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -9.68502 6.88431 0 0 -0 -0.455067 - - - - - - 5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -17.375 12.725 0 0 -0 0 - - - - - - 10.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 10.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -14.95 7.55 0 0 -0 -1.5708 - - - - - - 2.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 2.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -13.65 2.375 0 0 -0 0 - - - - - - 6.25 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 6.25 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 19.3304 1.86165 0 0 -0 3.14159 - - - - - - 8.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 8.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 16.2804 -2.31335 0 0 -0 -1.5708 - - - - - - 6.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 6.75 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 19.5804 -6.48835 0 0 -0 0 - - - - - - 2 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 2 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - -8.9507 7.45 0 0 -0 1.5708 - - - - - - 8.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 8.5 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 22.8804 -2.31335 0 0 -0 1.5708 - - - - - - 0.65 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 0.65 0.15 2.5 - - - - - 1 1 1 1 - - - 0 - - - 22.6304 1.86165 0 0 -0 3.14159 - - 1 - \ No newline at end of file diff --git a/src/rove_description/worlds/walls.sdf b/src/rove_description/worlds/walls.sdf new file mode 100644 index 0000000..9aa0019 --- /dev/null +++ b/src/rove_description/worlds/walls.sdf @@ -0,0 +1,869 @@ + + + + 0 0 0 0 0 0 + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + 0 + + + -12.4007 5.45 0 0 -0 1.5708 + + + + + + 1 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + 0 + + + -11.9757 8.375 0 0 -0 0 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -7.17535 9.4 0 0 -0 0.523599 + + + + + + 9.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 9.75 0.15 2.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + 0 + + + -0.6 10.425 0 0 -0 0 + + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.2 7.5 0 0 -0 -1.5708 + + + + + + 12 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 12 0.15 2.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + 0 + + + 4.2 -1.35 0 0 -0 -1.5708 + + + + + + 6.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 7.375 -7.275 0 0 -0 0 + + + + + + 14 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 14 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 10.55 -0.35 0 0 -0 1.5708 + + + + + + 17 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 17 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 18.975 6.575 0 0 -0 0 + + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 27.4 -2.85 0 0 -0 -1.5708 + + + + + + 30.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 30.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 12.1 -12.275 0 0 -0 3.14159 + + + + + + 15 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 15 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.2 -4.85 0 0 -0 1.5708 + + + + + + 1.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -10.985 7.80931 0 0 -0 -0.785398 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -5.25 2.575 0 0 -0 3.14159 + + + + + + 7.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 7.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -7.3 -1.1 0 0 -0 -1.5708 + + + + + + 11.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 11.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -13.1 -4.775 0 0 -0 3.14159 + + + + + + 4.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 4.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -18.9 -6.95 0 0 -0 -1.5708 + + + + + + 9 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 9 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -14.475 -9.125 0 0 -0 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -10.05 -10.925 0 0 -0 -1.5708 + + + + + + 17.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 17.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -18.725 -12.725 0 0 -0 3.14159 + + + + + + 13.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 13.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -27.4 -5.925 0 0 -0 1.5708 + + + + + + 7.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 7.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -23.6 0.875 0 0 -0 0 + + + + + + 12 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 12 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -19.8 6.8 0 0 -0 1.5708 + + + + + + 1.78502 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.78502 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.68502 6.88431 0 0 -0 -0.455067 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -17.375 12.725 0 0 -0 0 + + + + + + 10.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 10.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -14.95 7.55 0 0 -0 -1.5708 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -13.65 2.375 0 0 -0 0 + + + + + + 6.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 19.3304 1.86165 0 0 -0 3.14159 + + + + + + 8.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 8.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 16.2804 -2.31335 0 0 -0 -1.5708 + + + + + + 6.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 19.5804 -6.48835 0 0 -0 0 + + + + + + 2 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -8.9507 7.45 0 0 -0 1.5708 + + + + + + 8.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 8.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 22.8804 -2.31335 0 0 -0 1.5708 + + + + + + 0.65 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 0.65 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 22.6304 1.86165 0 0 -0 3.14159 + + 1 + + \ No newline at end of file diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index c290fad..319d890 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -224,7 +224,6 @@ global_costmap: origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 - max_obstacle_height: 2.0 unknown_threshold: 15 mark_threshold: 0 observation_sources: pointcloud @@ -268,7 +267,6 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 # radius set and used, so no footprint points - resolution: 0.05 plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -310,7 +308,6 @@ local_costmap: origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 - max_obstacle_height: 2.0 unknown_threshold: 15 mark_threshold: 0 observation_sources: pointcloud diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index 53857fd..69dd2d5 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/src/rove_navigation/launch/navigation.launch.py @@ -2,14 +2,15 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch_ros.actions import SetRemap def generate_launch_description(): - pkg_rove_navigation = get_package_share_directory('rove_navigation') navigation_launch_path = PathJoinSubstitution( @@ -18,15 +19,33 @@ def generate_launch_description(): nav2_config_path = os.path.join(pkg_rove_navigation, 'config', 'rove_nav_params.yaml') + + bt_xml_path = PathJoinSubstitution( + [pkg_rove_navigation, 'config', 'follow_dynamic_point.xml'] + ) + + person_following_node = Node( + package="rove_navigation", + executable='person_following', + name='person_following', + output='screen', + ) + nav = IncludeLaunchDescription( PythonLaunchDescriptionSource(navigation_launch_path), launch_arguments={ 'use_sim_time': 'true', 'params_file': nav2_config_path - }.items() + }.items(), ) - + return LaunchDescription([ - nav + person_following_node, + GroupAction( + actions=[ + SetRemap(src='cmd_vel', dst='nav_vel'), + nav, + ] + ) ]) diff --git a/src/rove_navigation/rove_navigation/behavior/__init__.py b/src/rove_navigation/rove_navigation/behavior/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_navigation/rove_navigation/behavior/navigation.py b/src/rove_navigation/rove_navigation/behavior/navigation.py new file mode 100644 index 0000000..0587ff1 --- /dev/null +++ b/src/rove_navigation/rove_navigation/behavior/navigation.py @@ -0,0 +1,110 @@ + +import py_trees +import transforms3d + +from action_msgs.msg import GoalStatus +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + +import numpy as np +from geometry_msgs.msg import PointStamped + +from rclpy.node import Node + + +class GetLocationFromQueue(py_trees.behaviour.Behaviour): + """Gets a location name from the queue""" + + def __init__(self, name, location_dict): + super(GetLocationFromQueue, self).__init__(name) + self.location_dict = location_dict + self.bb = py_trees.blackboard.Blackboard() + + def update(self): + """Checks for the status of the navigation action""" + loc_list = self.bb.get("loc_list") + if len(loc_list) == 0: + self.logger.info("No locations available") + return py_trees.common.Status.FAILURE + else: + target_location = loc_list.pop() + self.logger.info(f"Selected location {target_location}") + target_pose = self.location_dict[target_location] + self.bb.set("target_pose", target_pose) + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + self.logger.info(f"Terminated with status {new_status}") + + +class GoToPose(py_trees.behaviour.Behaviour): + """Wrapper behavior around the `move_base` action client""" + + def __init__(self, name, pose, node): + super(GoToPose, self).__init__(name) + self.pose = pose + self.client = None + self.node = node + self.bb = py_trees.blackboard.Blackboard() + + def initialise(self): + """Sends the initial navigation action goal""" + # Check if there is a pose available in the blackboard + try: + target_pose = self.bb.get("target_pose") + if target_pose is not None: + self.pose = target_pose + except: + pass + + self.client = ActionClient(self.node, NavigateToPose, "/navigate_to_pose") + self.client.wait_for_server() + + self.goal_status = None + x, y, theta = self.pose + self.logger.info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") + self.goal = self.create_move_base_goal(x, y, theta) + self.send_goal_future = self.client.send_goal_async(self.goal) + self.send_goal_future.add_done_callback(self.goal_callback) + + def goal_callback(self, future): + res = future.result() + if res is None or not res.accepted: + return + future = res.get_result_async() + future.add_done_callback(self.goal_result_callback) + + def goal_result_callback(self, future): + # If there is a result, we consider navigation completed and save the + # result code to be checked in the `update()` method. + self.goal_status = future.result().status + + def update(self): + """Checks for the status of the navigation action""" + # If there is a result, we can check the status of the action directly. + # Otherwise, the action is still running. + if self.goal_status is not None: + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.FAILURE + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + self.logger.info(f"Terminated with status {new_status}") + self.client = None + self.bb.set("target_pose", None) + + def create_move_base_goal(self, x, y, theta): + """Creates a MoveBaseGoal message from a 2D navigation pose""" + goal = NavigateToPose.Goal() + goal.pose.header.frame_id = "map" + goal.pose.header.stamp = self.node.get_clock().now().to_msg() + goal.pose.pose.position.x = x + goal.pose.pose.position.y = y + quat = transforms3d.euler.euler2quat(0, 0, theta) + goal.pose.pose.orientation.w = quat[0] + goal.pose.pose.orientation.x = quat[1] + goal.pose.pose.orientation.y = quat[2] + goal.pose.pose.orientation.z = quat[3] + return goal \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/behavior_square_pattroling.py b/src/rove_navigation/rove_navigation/behavior_square_pattroling.py new file mode 100644 index 0000000..2a14e52 --- /dev/null +++ b/src/rove_navigation/rove_navigation/behavior_square_pattroling.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +""" +Test script for running a simple behavior tree for autonomous navigation. +Taken from https://github.com/sea-bass/turtlebot3_behavior_demos/blob/main/tb3_autonomy/scripts/autonomy_node.py + + +Example usage: + ros2 run rove_behavior autonomy_node +""" + +import os +import yaml +import random +import rclpy +from rclpy.node import Node +import time +import py_trees +import py_trees_ros +from py_trees.common import OneShotPolicy +from ament_index_python.packages import get_package_share_directory + +from rove_navigation.behavior.navigation import GoToPose, GetLocationFromQueue + + +class AutonomyBehavior(Node): + def __init__(self): + super().__init__("autonomy_node") + self.get_logger().info("AutonomyBehavior node has been initialized.") + + # Defines object locations as [x, y, theta] + self.locations = { + "location1": [0.0, 0.0, -1.571], + "location2": [2.0, 0.0, 1.571], + "location3": [2.0, 2.0, 0.0], + "location4": [0.0, 2.0, 3.142], + } + + self.loc_list = list(self.locations.keys()) + + # Create and setup the behavior tree + self.tree = self.create_naive_tree() + + + def create_naive_tree(self): + """Create behavior tree with explicit nodes for each location.""" + seq = py_trees.composites.Sequence(name="navigation", memory=True) + root = py_trees.decorators.OneShot( + name="root", child=seq, policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION + ) + tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) + tree.setup(timeout=15.0, node=self) + + for loc in self.loc_list: + pose = self.locations[loc] + seq.add_child(GoToPose(f"go_to_{loc}", pose, self)) + self.get_logger().info(f"Added GoToPose node for {loc} with pose {pose}") + + return tree + + def execute(self, period=0.5): + """Executes the behavior tree at the specified period.""" + self.tree.tick_tock(period_ms=period * 1000.0) + rclpy.spin(self.tree.node) + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + behavior = AutonomyBehavior() + behavior.execute() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/green_person_tracker.py b/src/rove_navigation/rove_navigation/green_person_tracker.py new file mode 100755 index 0000000..cb8fa9f --- /dev/null +++ b/src/rove_navigation/rove_navigation/green_person_tracker.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import PointStamped +from cv_bridge import CvBridge +import cv2 +import numpy as np +from tf2_ros import Buffer, TransformListener +from tf2_geometry_msgs import do_transform_point + +class Tracker(Node): + def __init__(self): + super().__init__('tracker') + self.img_sub = self.create_subscription( + Image, + '/zed/zed_node/rgb/image_rect_color', + self.image_callback, + 10) + self.depth_sub = self.create_subscription( + Image, + '/zed/zed_node/depth/depth_registered', + self.depth_callback, + 10) + self.visualization_pub = self.create_publisher( + Image, + '/green_person_bounding_box', + 10) + self.position_pub = self.create_publisher( + PointStamped, + '/person_position', + 10) + + self.bridge = CvBridge() + self.depth_image = None + self.get_logger().info("Node started!") + + # Camera intrinsic parameters + self.fx = 363 # Focal length in x axis + self.fy = 363 # Focal length in y axis + self.cx = 672 # Optical center x coordinate + self.cy = 188 # Optical center y coordinate + + # TF2 Buffer and Listener + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + def depth_callback(self, msg): + try: + self.depth_image = self.bridge.imgmsg_to_cv2(msg, '32FC1') + except Exception as e: + self.get_logger().error(f"Failed to process depth image: {str(e)}") + + def image_callback(self, msg): + try: + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + frame = cv_image.copy() + + # Convert the frame to HSV color space + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + + # Define the range for green color in HSV (expanded range) + lower_green = np.array([30, 40, 40]) + upper_green = np.array([90, 255, 255]) + + # Create a mask for green color + mask = cv2.inRange(hsv, lower_green, upper_green) + + # Find contours in the mask + contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + if contours: + # Find the largest contour (assuming it is the object to track) + largest_contour = max(contours, key=cv2.contourArea) + x, y, w, h = cv2.boundingRect(largest_contour) + + # Draw bounding box + cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2) + + # Get the center of the bounding box + center_x = x + w // 2 + center_y = y + h // 2 + + # Publish the position if depth image is available + if self.depth_image is not None: + depth = float(self.depth_image[center_y, center_x]) + if np.isfinite(depth): + self.publish_position(center_x, center_y, depth, msg.header) + else: + self.get_logger().warn("No green object detected!") + cv2.putText(frame, "No green object detected", (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Convert the frame back to an Image message and publish it + img_msg = self.bridge.cv2_to_imgmsg(frame, 'bgr8') + self.visualization_pub.publish(img_msg) + + except Exception as e: + self.get_logger().error(f"Failed to process image: {str(e)}") + + def publish_position(self, x, y, depth, header): + # Convert pixel coordinates to camera coordinates + x_cam = (x - self.cx) * depth / self.fx + y_cam = (y - self.cy) * depth / self.fy + z_cam = depth + + point = PointStamped() + point.header = header + point.point.x = x_cam + point.point.y = y_cam + point.point.z = z_cam + + # Transform the point to the base_link frame + try: + transform = self.tf_buffer.lookup_transform('base_link', header.frame_id, rclpy.time.Time()) + point_transformed = do_transform_point(point, transform) + self.position_pub.publish(point_transformed) + self.get_logger().info(f"Person (green square) position in base_link: x={point_transformed.point.x}, y={point_transformed.point.y}, z={point_transformed.point.z}") + except Exception as e: + self.get_logger().error(f"Failed to transform point: {str(e)}") + +def main(args=None): + rclpy.init(args=args) + node = Tracker() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Node interrupted by user") + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/rove_navigation/rove_navigation/person_following.py b/src/rove_navigation/rove_navigation/person_following.py new file mode 100644 index 0000000..54d905b --- /dev/null +++ b/src/rove_navigation/rove_navigation/person_following.py @@ -0,0 +1,61 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PointStamped, PoseStamped +from tf2_ros import Buffer, TransformListener +from tf2_geometry_msgs import PointStamped +import math + +class NavigateToPersonNode(Node): + def __init__(self, truncate_distance): + super().__init__('navigate_to_person') + self.subscription = self.create_subscription( + PointStamped, + '/person_position', + self.navigate_to_person, + 10) + + self.goal_update_pub = self.create_publisher( + PoseStamped, + '/goal_pose', + 10) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Distance to truncate from the target position + self.truncate_distance = truncate_distance + + def navigate_to_person(self, msg): + # Ensure that the transformation is available + now = rclpy.time.Time() + if self.tf_buffer.can_transform('map', msg.header.frame_id, now): + point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1)) + goal_pose = PoseStamped() + goal_pose.header.frame_id = "map" + goal_pose.header.stamp = self.get_clock().now().to_msg() + + # Calculate the angle for the truncated goal using atan2 + angle = math.atan2(point_transformed.point.y, point_transformed.point.x) + + # Calculate the position truncating the specified distance from the transformed position + goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle) + goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle) + goal_pose.pose.position.z = 0.0 # Normally zero for ground robots + + goal_pose.pose.orientation.w = 1.0 # No rotation about the z-axis + + # Log the navigation target for debugging + self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}') + + # Publish the goal + self.goal_update_pub.publish(goal_pose) + +def main(args=None): + rclpy.init(args=args) + # You can adjust the truncate distance here + node = NavigateToPersonNode(truncate_distance=1.5) # for example, truncate 1.5 meters + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index 871b4ee..9e8d29f 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -24,6 +24,10 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'navigate_to_person = rove_navigation.navigate_to_person:main', + 'green_person_tracker = rove_navigation.green_person_tracker:main', + 'behavior_square_pattroling = rove_navigation.behavior_square_pattroling:main', + 'person_following = rove_navigation.person_following:main', ], }, ) diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 56c3cc8..c0c8684 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -2,7 +2,7 @@ ekf_filter_node_local: ros__parameters: frequency: 30.0 - two_d_mode: false + two_d_mode: true publish_tf: true map_frame: map odom_frame: odom @@ -18,9 +18,9 @@ ekf_filter_node_local: imu0: /imu imu0_config: [false, false, false, # x y z - false, false, true, # roll pitch yaw + true, true, true, # roll pitch yaw false, false, false, # vx vy vz - false, false, true, # vroll vpitch vyaw + true, true, true, # vroll vpitch vyaw false, false, false] # ax ay az imu0_queue_size: 20 diff --git a/src/rove_slam/launch/slam3d_full.launch.py b/src/rove_slam/launch/slam3d_full.launch.py index 4492c1a..70ba8ef 100644 --- a/src/rove_slam/launch/slam3d_full.launch.py +++ b/src/rove_slam/launch/slam3d_full.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): 'subscribe_depth':True, 'subscribe_odom_info':False, 'subscribe_scan_cloud': True, - 'subscribe_rgbd':True, + 'subscribe_rgb':True, 'approx_sync': True, 'publish_tf':True, 'Odom/Strategy':'0', @@ -81,7 +81,7 @@ def generate_launch_description(): output='screen', parameters=[{ 'max_clouds': 10, - 'fixed_frame_id':'', + 'fixed_frame_id':'sensor_link', 'use_sim_time': use_sim_time, }], remappings=[ @@ -121,5 +121,5 @@ def generate_launch_description(): camera_sync, point_cloud_assembler_node, rtabmap_node, - #rtabmap_viz_node + rtabmap_viz_node ]) diff --git a/src/rove_slam/launch/velodyne_3d.launch.py b/src/rove_slam/launch/velodyne_3d.launch.py index f0636e2..cb66d24 100644 --- a/src/rove_slam/launch/velodyne_3d.launch.py +++ b/src/rove_slam/launch/velodyne_3d.launch.py @@ -118,5 +118,5 @@ def generate_launch_description(): icp_odometry_node, point_cloud_assembler_node, rtabmap_node, - rtabmap_viz_node + #rtabmap_viz_node ]) diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index 58144ed..4391f4f 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -1,26 +1,27 @@ -# Parameters to override zed_wrapper parameters +# Parameters to override zed_wrapper parameters /**: - ros__parameters: - general: - grab_resolution: 'HD720' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' - pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images - grab_frame_rate: 30 # ZED SDK internal grabbing rate - mapping: - mapping_enabled: false # True to enable mapping and fused point cloud pubblication - resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f] - max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] - fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud - clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection - pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. - pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference - depth: - depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) - depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] - openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] - point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) - depth_confidence: 100 # [DYNAMIC] - depth_texture_conf: 100 # [DYNAMIC] - remove_saturated_areas: true # [DYNAMIC] + ros__parameters: + general: + grab_resolution: "VGA" # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' + pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images + grab_frame_rate: 30 # ZED SDK internal grabbing rate + camera_name: "zed" + mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. + pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference + depth: + depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 100 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] - min_depth: 0.2 # Min: 0.2, Max: 3.0 - max_depth: 10.0 # Max: 40.0 \ No newline at end of file + min_depth: 0.2 # Min: 0.2, Max: 3.0 + max_depth: 10.0 # Max: 40.0 diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py index cb78318..9e1c9bf 100644 --- a/src/rove_zed/launch/zed_mapping.launch.py +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -21,7 +21,12 @@ def generate_launch_description(): launch_description_source=launch_file_path, launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` ['camera_model', camera_model], - ['ros_params_override_path', config_override_path] + ['ros_params_override_path', config_override_path], + ['publish_tf', 'false'], + ['publish_map_tf', 'false'], + ['map_frame', 'map'], + ['odometry_frame', 'odom'], + ['use_sim_time', 'false'], ] ) ]) \ No newline at end of file