Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Gazebo support (draft) #19

Merged
merged 25 commits into from
Jun 6, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
61db58e
first step to gazebo support
mikaelarguedas Apr 2, 2019
3cdf7a1
add scripts taken and modifies from turtlebot3_simulation
mikaelarguedas Apr 2, 2019
806188c
add simulation launchfile
mikaelarguedas Apr 2, 2019
09cba9a
move urdf_spawner back in confbot_simulation
mikaelarguedas Apr 2, 2019
d164f5a
restore package:// path
mikaelarguedas Apr 2, 2019
7bb59d9
add dependencies not part of the nightly image
mikaelarguedas Apr 2, 2019
9b37f60
blacklist dependencies built from source from the test invocation
mikaelarguedas Apr 2, 2019
84cc7d9
fix test failures
mikaelarguedas Apr 2, 2019
1c73252
almost empty world
mikaelarguedas Apr 2, 2019
3aa543d
rebasing
mikaelarguedas Apr 3, 2019
60deaa4
dont set use_sim_time as currently doesn't impact time used
mikaelarguedas May 4, 2019
d7695e7
Revert "dont set use_sim_time as currently doesn't impact time used"
mikaelarguedas May 4, 2019
7742dc2
use libgazebo_ros_init to have sim time published
mikaelarguedas May 4, 2019
d5d09f4
fix conversion error from string to bool
Karsten1987 May 20, 2019
5427e4d
actually using the sim time parameter correctly
Karsten1987 May 20, 2019
739006f
interpret launch configuration as yaml
Karsten1987 May 21, 2019
ae5f961
visualize the laser scan in gazebo
Karsten1987 May 22, 2019
394521e
use sim time
Karsten1987 May 22, 2019
e885f50
make laser_frame point forward
mikaelarguedas Jun 5, 2019
6777de9
gazebo_ros now declares/set use_sim_time to true by default
mikaelarguedas Jun 5, 2019
63cfa77
add camera
mikaelarguedas Jun 5, 2019
ea88a55
move back camera_frame on the robot base
mikaelarguedas Jun 5, 2019
18e5594
add rviz config for gazebo
mikaelarguedas Jun 5, 2019
82127e1
add preliminary Gazebo section
mikaelarguedas Jun 5, 2019
cadb71a
fix config file name in instructions
mikaelarguedas Jun 5, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions additional_repos.repos
Original file line number Diff line number Diff line change
@@ -1 +1,13 @@
repositories:
gazebo_ros_pkgs:
type: git
url: https://github.com/ros-simulation/gazebo_ros_pkgs.git
version: ros2
image_common:
type: git
url: https://github.com/ros-perception/image_common.git
version: ros2
vision_opencv:
type: git
url: https://github.com/ros-perception/vision_opencv.git
version: ros2
43 changes: 43 additions & 0 deletions confbot_bringup/launch/confbot_state_publisher.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
# Copyright 2019 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# /* Author: Darby Lim */

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
urdf_file_name = 'confbot.urdf'
urdf = os.path.join(get_package_share_directory('confbot_description'), 'urdf', urdf_file_name)

return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true'),

Node(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
arguments=[urdf])
])
254 changes: 244 additions & 10 deletions confbot_description/urdf/confbot.urdf
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,19 @@
</material>

<link name="base_link">
<inertial>
<mass value="5"/>
<!--inertia ixx="0.4" ixy="0" ixz="0" iyy="0.4" iyz="0" izz="0.8"/-->
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 0.1"/>
<!--mesh filename="package://confbot_description/meshes/confbot.dae"/-->
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
Expand All @@ -19,11 +32,23 @@
</visual>
</link>
<joint name="laser_frame" type="fixed">
<origin rpy="0 0 -1.57" xyz="0.45 0.0 0.1"/>
<origin rpy="0 0 0" xyz="0.45 0.0 0.1"/>
<parent link="base_link"/>
<child link="laser_link"/>
<child link="laser_frame"/>
</joint>
<link name="laser_link">
<link name="laser_frame">
<inertial>
<mass value="1"/>
<inertia ixx="1.0e-03" ixy="0" ixz="0" iyy="1.0e-3" iyz="0" izz="1.0e-3"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="black"/>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
Expand All @@ -32,33 +57,242 @@
<material name="black"/>
</visual>
</link>
<joint name="wheel_l_frame" type="fixed">
<origin rpy="0 0 0" xyz="0 0.6 0.0"/>
<joint name="camera_frame" type="fixed">
<origin rpy="0 0 0" xyz="0.4 0.0 0.2"/>
<parent link="base_link"/>
<child link="camera_frame"/>
</joint>
<link name="camera_frame">
<inertial>
<mass value="1"/>
<inertia ixx="1.0e-03" ixy="0" ixz="0" iyy="1.0e-3" iyz="0" izz="1.0e-3"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="black"/>
</collision>
</link>
<joint name="camera_optical_fixed_joint" type="fixed">
<origin rpy="0 1.57079632679 0" xyz="0 0 0"/>
<parent link="camera_frame"/>
<child link="camera_optical_frame"/>
</joint>
<link name="camera_optical_frame"/>
<joint name="wheel_l_joint" type="continuous">
<origin rpy="0 0 0" xyz="0 0.55 0.0"/>
<axis xyz="0 1.0 0"/>
<parent link="base_link"/>
<child link="wheel_l_link"/>
</joint>
<link name="wheel_l_link">
<inertial>
<mass value="1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
<!--inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/-->
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.1" length="0.05"/>
</geometry>
<material name="white"/>
</collision>
<visual>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.3" length="0.2"/>
<cylinder radius="0.1" length="0.05"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="wheel_r_frame" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.6 0.0"/>
<joint name="wheel_r_joint" type="continuous">
<origin rpy="0 0 0" xyz="0 -0.55 0.0"/>
<axis xyz="0 1.0 0"/>
<parent link="base_link"/>
<child link="wheel_r_link"/>
</joint>
<link name="wheel_r_link">
<inertial>
<mass value="1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.1" length="0.05"/>
</geometry>
<material name="white"/>
</collision>
<visual>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.3" length="0.2"/>
<cylinder radius="0.1" length="0.05"/>
</geometry>
<material name="white"/>
</visual>
</link>
</robot>
<joint name="caster_wheel_front_joint" type="continuous">
<origin rpy="0 0 0" xyz="0.4 0.0 -0.05"/>
<axis xyz="1.0 1.0 0"/>
<parent link="base_link"/>
<child link="caster_wheel_front_link"/>
</joint>
<link name="caster_wheel_front_link">
<inertial>
<mass value="1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<!--origin rpy="1.57 0 0" xyz="0 0 0"/-->
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="white"/>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="caster_wheel_rear_joint" type="continuous">
<origin rpy="0 0 0" xyz="-0.4 0.0 -0.05"/>
<axis xyz="1.0 1.0 0"/>
<parent link="base_link"/>
<child link="caster_wheel_rear_link"/>
</joint>
<link name="caster_wheel_rear_link">
<inertial>
<mass value="1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
<collision>
<!--origin rpy="1.57 0 0" xyz="0 0 0"/-->
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="white"/>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="white"/>
</visual>
</link>
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>

<ros>
<namespace>/</namespace>
<argument>cmd_vel:=cmd_vel</argument>
<argument>odom:=odom</argument>
</ros>

<!-- wheels -->
<left_joint>wheel_l_joint</left_joint>
<right_joint>wheel_r_joint</right_joint>

<!-- kinematics -->
<wheel_separation>1.2</wheel_separation>
<wheel_diameter>0.6</wheel_diameter>

<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>

<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>

<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>

</plugin>
</gazebo>

<gazebo reference="laser_frame">
<!--sensor type="gpu_ray" name="lidar"-->
<sensor type="ray" name="lidar">
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm using ray here because gpu_ray returns infinity for all points even the one hitting obstacles.
also it prints this warning:

[gazebo-1] [Wrn] [msgs.cc:1852] Conversion of sensor type[gpu_ray] not supported.

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That warning can be ignored.

Does your model have visuals besides collisions? The gpu_ray sees visuals, while the ray sees collisions.

Can you reproduce the issue with this demo world? https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ros2/gazebo_plugins/worlds/gazebo_ros_ray_sensor_demo.world

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm using only primitive shapes that I dropped in the world from the Gazebo UI, I think they have both visual and collision.

Thanks for the pointer, we'll try it on the demo world to see if it's the same.

Note: I'm compiling the gazebo plugins from source without any specific flags, maybe GPU support is not enabled?

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think they have both visual and collision.

Yup, they do

I'm compiling the gazebo plugins from source without any specific flags, maybe GPU support is not enabled?

There are no flags to disable GPU support, so compilation should be working correctly. At runtime, if gzclient works, then I'd expect the rendering sensors to work as well. You could double-check by dropping a camera in the world and seeing if the images show the shapes.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@chapulina Sorry for the delay, I fInally got a chance to try the demo world (using the dashing debs), and I'm facing the same issue:

command ran:

gazebo --verbose /opt/ros/dashing/share/gazebo_plugins/worlds/gazebo_ros_ray_sensor_demo.world```
Regular ray sensor works (some non inf values)
$ ros2 topic echo /ray/laserscan
header:
  stamp:
    sec: 13
    nanosec: 25000000
  frame_id: ray_link
angle_min: -0.5235999822616577
angle_max: 0.5235999822616577
angle_increment: 0.0035023412201553583
time_increment: 0.0
scan_time: 0.0
range_min: 0.05000000074505806
range_max: 50.0
ranges: [.inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 2.0448575019836426, 2.0313127040863037, 2.0236432552337646, 2.018503189086914, 2.0150201320648193, 2.0128238201141357, 2.0117366313934326, 2.0116827487945557, 2.0126583576202393, 2.0147311687469482, 2.0180652141571045, 2.0230026245117188, 2.030329465866089, 2.0427944660186768, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 1.455374836921692, 1.45494544506073, 1.4545341730117798, '...']
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 100.0, 100.0, '...']



gpu_ray sensor returns only infinity
$ ros2 topic echo /gpu_ray/laserscan
header:
  stamp:
    sec: 7
    nanosec: 0
  frame_id: ray_link
angle_min: -0.5235999822616577
angle_max: 0.5235999822616577
angle_increment: 0.0035023412201553583
time_increment: 0.0
scan_time: 0.0
range_min: 0.05000000074505806
range_max: 50.0
ranges: [-.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, -.inf, '...']
intensities: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, '...']

I tried the camera demo and can see the objects successfully:

gazebo --verbose  /opt/ros/dashing/share/gazebo_plugins/worlds/gazebo_ros_camera_demo.world

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Interesting, I get very similar readings from both of them, I'm on a source install.

$ ros2 topic echo /gpu_ray/laserscan
header:
  stamp:
    sec: 139
    nanosec: 2000000
  frame_id: ray_link
angle_min: -0.5235999822616577
angle_max: 0.5235999822616577
angle_increment: 0.0035023412201553583
time_increment: 0.0
scan_time: 0.0
range_min: 0.05000000074505806
range_max: 50.0
ranges: [.inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 2.0503170490264893, 2.033238172531128, 2.024780511856079, 2.0192136764526367, 2.0154314041137695, 2.012951612472534, 2.011847734451294, 2.0118982791900635, 2.0129361152648926, 2.0150675773620605, 2.0186455249786377, 2.0241785049438477, 2.032437562942505, 2.0490212440490723, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 1.454956293106079, 1.4545066356658936, '...']
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, '...']
---
$ ros2 topic echo /ray/laserscan
header:
  stamp:
    sec: 153
    nanosec: 18000000
  frame_id: ray_link
angle_min: -0.5235999822616577
angle_max: 0.5235999822616577
angle_increment: 0.0035023412201553583
time_increment: 0.0
scan_time: 0.0
range_min: 0.05000000074505806
range_max: 50.0
ranges: [.inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 2.0448575019836426, 2.0313127040863037, 2.0236432552337646, 2.018503189086914, 2.0150201320648193, 2.0128238201141357, 2.0117366313934326, 2.0116827487945557, 2.0126583576202393, 2.0147311687469482, 2.0180652141571045, 2.0230026245117188, 2.030329465866089, 2.0427944660186768, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, .inf, 1.455374836921692, 1.45494544506073, 1.4545341730117798, '...']
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, 100.0, 100.0, '...']
---

Do the GPU readings look right on Gazebo's topic viewer (Ctrl + T, and you need to zoom in):

topicviewer

I can only think it's an issue with your graphics, but I'm not sure why the RGB camera works. Are the point cloud messages working?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can only think it's an issue with your graphics, but I'm not sure why the RGB camera works.

Yeah I had some issues of RViz being very slow as well. I ended up reinstalling all the graphics drivers and it seems to have fixed the issue !

Thanks for taking the time to reproduce 🙇‍♂️ !

<visualize>true</visualize>
<update_rate>3</update_rate>
<ray>
<scan>
<horizontal>
<samples>1875</samples>
<resolution>1</resolution>
<min_angle>-2.0943951023931953</min_angle>
<max_angle>2.0943951023931953</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>30.0</max>
<resolution>0.001</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.008</stddev>
</noise>
</ray>
<!--plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_gpu_laser.so"-->
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_ray_sensor.so">
<output_type>sensor_msgs/LaserScan</output_type>
<ros>
<!--frameName>/laser_frame</frameName-->
<namespace>/</namespace>
<argument>~/out:=scan</argument>
</ros>
</plugin>
</sensor>
</gazebo>

<gazebo reference="camera_frame">
<sensor type="camera" name="confbot_camera">
<update_rate>30</update_rate>
<visualize>true</visualize>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<frame_name>camera_optical_frame</frame_name>
<!-- camera_name>omit so it defaults to sensor name</camera_name-->
<!-- frame_name>omit so it defaults to link name</frameName-->
</plugin>
</sensor>
</gazebo>


</robot>
17 changes: 17 additions & 0 deletions confbot_simulation/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
cmake_minimum_required(VERSION 3.5)
project(confbot_simulation)

# find dependencies
find_package(ament_cmake REQUIRED)

install(DIRECTORY config launch world
DESTINATION share/${PROJECT_NAME})
install(PROGRAMS ${PROJECT_NAME}/urdf_spawner.py
DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
Loading