diff --git a/dockerfiles/install_scripts/core_packages.sh b/dockerfiles/install_scripts/core_packages.sh index 632afaa9..035cf28c 100755 --- a/dockerfiles/install_scripts/core_packages.sh +++ b/dockerfiles/install_scripts/core_packages.sh @@ -31,8 +31,7 @@ apt install -y \ ros-$ROS_DISTRO-ros2-controllers \ ros-$ROS_DISTRO-rqt-tf-tree \ ros-$ROS_DISTRO-slam-toolbox \ - ros-$ROS_DISTRO-velodyne \ - ros-$ROS_DISTRO-velodyne-description + ros-$ROS_DISTRO-pointcloud-to-laserscan pip install uv --break-system-packages diff --git a/dockerfiles/install_scripts/velodyne_ros.sh b/dockerfiles/install_scripts/velodyne_ros.sh new file mode 100755 index 00000000..19c7be76 --- /dev/null +++ b/dockerfiles/install_scripts/velodyne_ros.sh @@ -0,0 +1,23 @@ +#!/bin/bash -i + +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +set -e +source /home/$UNAME/.bashrc +apt update + +apt install -y \ + libpcap-dev \ + ros-$ROS_DISTRO-velodyne-description + +mkdir -p /home/$UNAME/velodyne_ws/src +cd /home/$UNAME/velodyne_ws +git clone -b ros2 https://github.com/alliander-opensource/velodyne.git # In our fork we use the Sensor Data QOS profile. + +rosdep update --rosdistro $ROS_DISTRO +rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y + +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +echo "source /home/$UNAME/velodyne_ws/install/setup.bash" >>/home/$UNAME/.bashrc \ No newline at end of file diff --git a/dockerfiles/rcdt_robotics.Dockerfile b/dockerfiles/rcdt_robotics.Dockerfile index c8bfe69c..928bc320 100644 --- a/dockerfiles/rcdt_robotics.Dockerfile +++ b/dockerfiles/rcdt_robotics.Dockerfile @@ -14,6 +14,9 @@ RUN ./franka_ros2.sh COPY ./install_scripts/husarion_ugv_ros.sh . RUN ./husarion_ugv_ros.sh +COPY ./install_scripts/velodyne_ros.sh . +RUN ./velodyne_ros.sh + COPY ./install_scripts/franka_lock_unlock.sh . RUN ./franka_lock_unlock.sh diff --git a/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py b/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py index f8b4410b..6447caf9 100644 --- a/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py +++ b/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py @@ -66,7 +66,6 @@ def launch_setup(context: LaunchContext) -> list: if "velodyne" in robots: bridge_topics.extend( [ - "/velodyne/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan", "/velodyne/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", ] ) diff --git a/ros2_ws/src/rcdt_sensors/launch/velodyne.launch.py b/ros2_ws/src/rcdt_sensors/launch/velodyne.launch.py index c430d7b7..f4d976fa 100644 --- a/ros2_ws/src/rcdt_sensors/launch/velodyne.launch.py +++ b/ros2_ws/src/rcdt_sensors/launch/velodyne.launch.py @@ -46,6 +46,7 @@ def launch_setup(context: LaunchContext) -> list: output="both", parameters=[ { + "model": "VLP16", "device_ip": "10.15.20.5", "frame_id": frame_prefix + "velodyne", } @@ -63,7 +64,7 @@ def launch_setup(context: LaunchContext) -> list: "velodyne_pointcloud", ["params"], "VLP16db.yaml" ), "model": "VLP16", - "min_range": 0.9, + "min_range": 0.1, "max_range": 130.0, } ], @@ -71,20 +72,6 @@ def launch_setup(context: LaunchContext) -> list: namespace=namespace, ) - velodyne_laserscan_node = Node( - package="velodyne_laserscan", - executable="velodyne_laserscan_node", - output="both", - parameters=[ - { - "ring": -1, - "resolution": 0.007, - } - ], - remappings=[("velodyne_points", "scan/points")], - namespace=namespace, - ) - static_transform_publisher = Node( package="tf2_ros", executable="static_transform_publisher", @@ -94,20 +81,35 @@ def launch_setup(context: LaunchContext) -> list: "--child-frame-id", "velodyne/base_link", "--x", - "0.13", + "0.0", "--y", - "-0.13", + "-0.06", "--z", "0.55", ], ) + pointcloud_to_laserscan_node = Node( + package="pointcloud_to_laserscan", + executable="pointcloud_to_laserscan_node", + remappings=[("cloud_in", "/velodyne/scan/points"), ("scan", "/velodyne/scan")], + parameters=[ + { + "target_frame": "panther/base_footprint", + "min_height": 0.1, + "max_height": 2.0, + "range_min": 0.05, + "range_max": 100.0, + } + ], + ) + return [ Register.on_start(robot_state_publisher, context), Register.on_start(static_transform_publisher, context), Register.on_start(velodyne_driver_node, context) if not use_sim else SKIP, Register.on_start(velodyne_transform_node, context) if not use_sim else SKIP, - Register.on_start(velodyne_laserscan_node, context) if not use_sim else SKIP, + Register.on_start(pointcloud_to_laserscan_node, context), ] diff --git a/ros2_ws/src/rcdt_utilities/rviz/panther_collision_monitor.rviz b/ros2_ws/src/rcdt_utilities/rviz/panther_collision_monitor.rviz index 6bf63e86..7ccda2a5 100644 --- a/ros2_ws/src/rcdt_utilities/rviz/panther_collision_monitor.rviz +++ b/ros2_ws/src/rcdt_utilities/rviz/panther_collision_monitor.rviz @@ -2,7 +2,6 @@ # # SPDX-License-Identifier: Apache-2.0 - Panels: - Class: rviz_common/Displays Help Height: 78 @@ -11,8 +10,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /LaserScan1/Topic1 Splitter Ratio: 0.5 - Tree Height: 752 + Tree Height: 728 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -30,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: LaserScan Visualization Manager: Class: "" Displays: @@ -242,6 +242,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /velocity_polygon_stop Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /velodyne/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -288,33 +322,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.972400188446045 + Distance: 16.111764907836914 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.07419729232788086 - Y: 0.12190007418394089 - Z: -0.8994024991989136 + X: -0.6480562686920166 + Y: 0.0014165602624416351 + Z: -0.9144597053527832 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8653979301452637 + Pitch: 0.22039839625358582 Target Frame: Value: Orbit (rviz) - Yaw: 0.8653979301452637 + Yaw: 4.913583278656006 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1019 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000361000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000361fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000361000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -324,5 +358,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 318 - Y: 0 + X: 0 + Y: 32 diff --git a/ros2_ws/src/rcdt_utilities/rviz/panther_navigation.rviz b/ros2_ws/src/rcdt_utilities/rviz/panther_navigation.rviz index 7f4aebd9..a1ed8a0b 100644 --- a/ros2_ws/src/rcdt_utilities/rviz/panther_navigation.rviz +++ b/ros2_ws/src/rcdt_utilities/rviz/panther_navigation.rviz @@ -10,6 +10,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /LaserScan1/Topic1 Splitter Ratio: 0.5 Tree Height: 752 - Class: rviz_common/Selection @@ -29,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: LaserScan Visualization Manager: Class: "" Displays: @@ -190,39 +191,6 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /velodyne/scan/points - Use Fixed Frame: true - Use rainbow: true - Value: true - Alpha: 0.699999988079071 Binary representation: false Binary threshold: 100 @@ -297,6 +265,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /plan Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /velodyne/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -343,25 +345,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 13.885334968566895 + Distance: 13.577466011047363 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.1741686761379242 - Y: 0.5766842365264893 - Z: -1.3313781023025513 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8653979301452637 + Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 0.8653979301452637 + Yaw: 3.140000104904175 Saved: ~ Window Geometry: Displays: @@ -379,5 +381,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 328 + X: 333 Y: 0 diff --git a/ros2_ws/src/rcdt_utilities/rviz/panther_navigation_and_collision_monitor.rviz b/ros2_ws/src/rcdt_utilities/rviz/panther_navigation_and_collision_monitor.rviz index 3d19ec61..3be6b162 100644 --- a/ros2_ws/src/rcdt_utilities/rviz/panther_navigation_and_collision_monitor.rviz +++ b/ros2_ws/src/rcdt_utilities/rviz/panther_navigation_and_collision_monitor.rviz @@ -10,6 +10,11 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /PointCloud21 + - /Map1 + - /Map1/Status1 + - /LaserScan1 + - /LaserScan1/Topic1 Splitter Ratio: 0.5 Tree Height: 752 - Class: rviz_common/Selection @@ -166,10 +171,10 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 148 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 @@ -182,11 +187,11 @@ Visualization Manager: Depth: 5 Durability Policy: Volatile History Policy: Keep Last - Reliability Policy: Reliable + Reliability Policy: Best Effort Value: /velodyne/scan/points Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 0.699999988079071 Binary representation: false Binary threshold: 100 @@ -287,10 +292,44 @@ Visualization Manager: Reliability Policy: Reliable Value: /velocity_polygon_stop Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /velodyne/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: panther/odom + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -333,25 +372,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 11.211273193359375 + Distance: 13.550743103027344 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.07419729232788086 - Y: 0.12190007418394089 - Z: -0.8994024991989136 + X: 0.7811704874038696 + Y: 1.2233942747116089 + Z: -2.024574041366577 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8653979301452637 + Pitch: 0.9403978586196899 Target Frame: Value: Orbit (rviz) - Yaw: 0.8653979301452637 + Yaw: 6.113586902618408 Saved: ~ Window Geometry: Displays: @@ -370,4 +409,4 @@ Window Geometry: collapsed: false Width: 1920 X: 318 - Y: 0 + Y: 32 diff --git a/ros2_ws/src/rcdt_utilities/rviz/panther_velodyne.rviz b/ros2_ws/src/rcdt_utilities/rviz/panther_velodyne.rviz index fa26c640..e406cc3e 100644 --- a/ros2_ws/src/rcdt_utilities/rviz/panther_velodyne.rviz +++ b/ros2_ws/src/rcdt_utilities/rviz/panther_velodyne.rviz @@ -10,6 +10,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /LaserScan1/Status1 Splitter Ratio: 0.5 Tree Height: 752 - Class: rviz_common/Selection @@ -69,6 +70,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false base_link: Alpha: 1 Show Axes: false @@ -82,11 +87,19 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + fl_wheel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false fl_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true + fr_wheel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false fr_wheel_link: Alpha: 1 Show Axes: false @@ -108,15 +121,27 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false rear_bumper_link: Alpha: 1 Show Axes: false Show Trail: false + rl_wheel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false rl_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true + rr_wheel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false rr_wheel_link: Alpha: 1 Show Axes: false @@ -153,7 +178,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - lidar: + velodyne: Alpha: 1 Show Axes: false Show Trail: false @@ -193,13 +218,46 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /velodyne/scan/points Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /velodyne/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -246,7 +304,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.152005672454834 + Distance: 9.76075267791748 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -261,10 +319,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7303981781005859 + Pitch: 0.2403986155986786 Target Frame: Value: Orbit (rviz) - Yaw: 0.9753978252410889 + Yaw: 1.4703973531723022 Saved: ~ Window Geometry: Displays: @@ -283,4 +341,4 @@ Window Geometry: collapsed: false Width: 1920 X: 328 - Y: 0 + Y: 32