Open ruicao-github opened 6 years ago
You will have to modify 3dsensor.launch too. It calls kinect.launch.xml with some arguments that it doesn't need anymore in your modified version, it is why you have unused args
error. You would also need to remap the depth image topic sent to depthimage_to_laserscan
.
You could also make a copy of 3dsensor.launch called kinect2sensor.launch for example, launching directly kinect2 inside and remap the correct topics/frames of depthimage_to_laserscan. Then replace 3dsensor.launch in demo_turtlebot_mapping.launch to kinect2sensor.launch. You may have to remap camera topics in demo_turtlebot_mapping.launch too.
You may have to verify if in the included move_base configuration, it uses topic from the camera directly. I think it uses only the fake scan generated but just to verify.
cheers, Mathieu
Hi,
thank you for the really helpful comment. It solves the problem that I have previously. Now the Kinect2 can communicate with the Turtlebot. However new problem comes....
I get this error after excuting roslaunch rtabmap_ros demo_turtlebot_mapping.launch
I don't know why the the dimension of the RGB image has to be integer multiple of the IR image ([rgb=960x540 depth=512x424]). With the same image resolution ratio, I could make the RTABmap standalone package to work. However, here it does not work.
While ignoring this error, I get the following warning:
I did checked the topics that are published, by runing rostopic list
:
/camera/bond /camera/hd/camera_info /camera/hd/image_color /camera/hd/image_color/compressed /camera/hd/image_color_rect /camera/hd/image_color_rect/compressed /camera/hd/image_depth_rect /camera/hd/image_depth_rect/compressed /camera/hd/image_mono /camera/hd/image_mono/compressed /camera/hd/image_mono_rect /camera/hd/image_mono_rect/compressed /camera/hd/points /camera/qhd/camera_info /camera/qhd/image_color /camera/qhd/image_color/compressed /camera/qhd/image_color_rect /camera/qhd/image_color_rect/compressed /camera/qhd/image_depth_rect /camera/qhd/image_depth_rect/compressed /camera/qhd/image_mono /camera/qhd/image_mono/compressed /camera/qhd/image_mono_rect /camera/qhd/image_mono_rect/compressed /camera/qhd/points /camera/sd/camera_info /camera/sd/image_color_rect /camera/sd/image_color_rect/compressed /camera/sd/image_depth /camera/sd/image_depth/compressed /camera/sd/image_depth_rect /camera/sd/image_depth_rect/compressed /camera/sd/image_ir /camera/sd/image_ir/compressed /camera/sd/image_ir_rect /camera/sd/image_ir_rect/compressed /camera/sd/points /map /move_base/current_goal /move_base/goal /move_base_simple/goal /navigation_velocity_smoother/raw_cmd_vel /rosout /rosout_agg /rtabmap/cloud_ground /rtabmap/cloud_map /rtabmap/cloud_obstacles /rtabmap/global_path /rtabmap/global_pose /rtabmap/goal /rtabmap/goal_node /rtabmap/goal_out /rtabmap/goal_reached /rtabmap/info /rtabmap/labels /rtabmap/local_path /rtabmap/mapData /rtabmap/mapGraph /rtabmap/mapPath /rtabmap/move_base/cancel /rtabmap/move_base/feedback /rtabmap/move_base/goal /rtabmap/move_base/result /rtabmap/move_base/status /rtabmap/octomap_binary /rtabmap/octomap_empty_space /rtabmap/octomap_full /rtabmap/octomap_grid /rtabmap/octomap_occupied_space /rtabmap/odom /rtabmap/odom_info /rtabmap/odom_last_frame /rtabmap/odom_local_map /rtabmap/odom_local_scan_map /rtabmap/proj_map /rtabmap/scan_map /rtabmap/user_data_async /scan /tf /tf_static
So the required topics are indeed published. I don't know what the error is...
This is the result in the visualisation in RVIZ (basically blank):
I also attached my modified launch files for the demo_turtlebot_mapping.launch
and kinect2sensor.launch
in the zip file, in case you want to check the settings:
launchFiles.zip
Again, thank you very much for your time!
The assert makes rtabmap node crashing, so rtabmap is not running anymore. rtabmap needs registered depth image to RGB camera, which in general RGB and depth has the same size. You should use all "qhd" topics or all "sd" topics, not a mix of both. With kinect v2, it is preferred to use all "qhd" topics to have better 3D point cloud reconstruction without black borders.
cheers, Mathieu
Hi,
thank you again for your generous help. Originally I thought the IR image is maximum only up to sd quality, so I made the RGB image to qhd and IR iamge to sd. Anyway, now after I change every image to qhd, the problem goes. However, I got new problem:
For the first warning from the image, I checked each topic with rostopic hz ...
and found out that, only the /scan
topic is not publishing any data, all other topics are publishing at normal rate. I did map the kinect2/qhd/image_depth_rect
to depthimage_to_laserscan
in my kinect2sensor.launch
file. However, either I am doing it incorrectly or there are some error elsewhere that prohibits the publish of the /scan topic.
I did some changes to my last launch files, so I attach it here in the zip file for your reference. launch.zip
The second warning that I get is that the transform from base_footprint to map is not available. I have no clue on what to do for this warning...
Here I attach the TF frames: frames.pdf
Thank you for your time and help!
From this file:
<launch>
<!-- "camera" should uniquely identify the device. All topics are pushed down
into the "camera" namespace, and it is prepended to tf frame ids. -->
<arg name="camera" default="kinect2"/>
<arg name="publish_tf" default="true"/>
<!-- <arg name="jpeg_quality" default="50"/>-->
<arg name="depth_method" default="opengl"/>
<arg name="scan_processing" default="true"/>
<arg name="depth_registration" default="true"/>
<arg if="$(arg depth_registration)" name="depth" value="depth_registered" />
<arg unless="$(arg depth_registration)" name="depth" value="depth" />
<!-- Laserscan topic -->
<arg name="scan_topic" default="scan"/>
<include file="/home/rui/catkin_ws/src/iai_kinect2/kinect2_bridge/launch/kinect2_bridge.launch">
<arg name="base_name" value="$(arg camera)"/>
<arg name="publish_tf" value="$(arg publish_tf)"/>
<!-- <arg name="jpeg_quality" value="$(arg jpeg_quality)"/>-->
<arg name="depth_method" value="$(arg depth_method)"/>
</include>
<remap from="qhd/image_depth_rect" to="depthimage_to_laserscan"/>
<!-- Laserscan
This uses lazy subscribing, so will not activate until scan is requested.
-->
<group if="$(arg scan_processing)">
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet $(arg camera)/$(arg camera)_nodelet_manager">
<!-- Pixel rows to use to generate the laserscan. For each column, the scan will
return the minimum value for those pixels centered vertically in the image. -->
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="/$(arg camera)_depth_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="$(arg camera)/$(arg depth)/image_raw"/>
<remap from="scan" to="$(arg scan_topic)"/>
<!-- Somehow topics here get prefixed by "$(arg camera)" when not inside an app namespace,
so in this case "$(arg scan_topic)" must provide an absolute topic name (issue #88).
Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 -->
<remap from="$(arg camera)/image" to="$(arg camera)/$(arg depth)/image_raw"/>
<remap from="$(arg camera)/scan" to="$(arg scan_topic)"/>
</node>
</group>
</launch>
Review the topics and parameters set to depthimage_to_laserscan for kinect2 topics and frames. For example, it may look like this:
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="standalone depthimage_to_laserscan/DepthImageToLaserScanNodelet">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="kinect2_rgb_optical_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="/kinect2/qhd/image_depth_rect"/>
<remap from="scan" to="/scan"/>
</node>
I set also a standlaone nodelet as I am not sure what was the name of kinect2 nodelet manager. You may load the nodelet on the kinect2 nodelet manager to save some serialization overhead.
cheers, Mathieu
Hi again,
thank you very much, the problem is solved!
However, a new problem comes up. Now I can do the mapping and see the point cloud and costmap in Rviz. However, the point cloud and the cost map and the laser scan are not aligned, as seen in this picture:
The yellow circled thing is the fake 2d laser scan (I believe). Notice it is vertical to the cost map. On the other hand, the point cloud is also about 90° in the yaw axis from the cost map. How can I align the three?
Also, on the command line, this is the most frequent warning that I get:
[ WARN] [1523043422.517376705]: Costmap2DROS transform timeout. Current time: 1523043422.5173, global_pose stamp: 1523043421.9541, tolerance: 0.5000
[ WARN] [1523043422.517497798]: Unable to get starting pose of robot, unable to create global plan
Very thank you for your time!
For the scan, maybe it is related to output_frame_id. You may want to show TF in RVIZ to debug TF.
What is your fixed frame in global options of RVIZ? it should be /map to correctly see the data. Maybe there is a TF rotation missing somewhere.
Hi,
thank you for the reply.
I did check the TF in Rviz. In the image I applied a rotation to the kinect2link such that the point cloud is on the ground, but the scan is vertical to the ground, like this.
This is the rotation that I defined for the kinect2link.
<arg name="pi/2" value="0"/>
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="base_link1"
args="$(arg optical_rotate) base_link kinect2_link 100" />
When I do not apply the rotation, the point cloud will be floating in the air above the robot, but the scan will be on the ground and looks normal:
Since the scan is generated from the kinect2_rgb_optical_frame, it is thus coupled with the kinect2 frame. If I rotate the kinect2, then the scan will also be rotated. Thus, if I make one to be in the normal orientation, the other will be in wrong orientation. If I make one on the ground, the other one will be vertical to the ground or above the robot.
I tried to rotate the scan topic by
<node pkg="tf" type="static_transform_publisher" name="base_link1"
args="$(arg optical_rotate) base_link kinect2_link 100" />
but the scan did not get rotated in the Rviz.
This is my current TF tree: frames.pdf
Maybe you have more insight in how to solve such problem?
Thank you for your time!
Adjust the <param name="output_frame_id" value="kinect2_rgb_optical_frame"/>
of the scan so that it matches the point cloud orientation. Maybe it should be base_link or kinect2_link.
Hi,
thank you for the useful comment. It is working! Now I have a problem in the navigation mode where the costmap cannot be constructed. The warning is like this:
We tried to set wait_for_transform_duration
to 0.2 on the command line as roslaunch rtabmap_ros demo_turtlebot_mapping.launch localization:=true wait_for_transform_duration:=0.2
but we get the same warning as the timeout
is still 0.1. So how can I change the timeout
?
Do you think solving timeout will fix the costmap problem?
We also find out that our odom
topic is empty:
Is that also a problem? And how to solve it?
Thank you for your time and help!
You may use rqt_console to know exactly from which node the warning message is coming. I think it is not from rtabmap, but from move_base's costmap update. For move_base, odom should be published at least on TF (/odom -> /base_footprint). rtabmap won't publish /map->/odom if it is not receiving odom either.
Hi, we manage to fix all the software related problems. Now trying to move our robot by setting the 2D Nav Goal in the Rviz. Our controller for motors accept PWM value. If Rtabmap or the Turtlebot publishes any velocity values, then we can map it to the PWM value. However, we don’t know which topic to subscribe to, to get the velocity values. We do not have the geometry_msgs/Twist topic.
These are the topics published:
/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/switch
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
/depthimage_to_laserscan/parameter_descriptions
/depthimage_to_laserscan/parameter_updates
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/initialpose
/joint_states
/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/kobuki_safety_controller/disable
/kobuki_safety_controller/enable
/kobuki_safety_controller/reset
/laptop_charge
/map
/map_updates
/mobile_base/commands/controller_info
/mobile_base/commands/digital_output
/mobile_base/commands/external_power
/mobile_base/commands/led1
/mobile_base/commands/led2
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/sound
/mobile_base/commands/velocity
/mobile_base/controller_info
/mobile_base/debug/raw_control_command
/mobile_base/debug/raw_data_command
/mobile_base/debug/raw_data_stream
/mobile_base/events/bumper
/mobile_base/events/button
/mobile_base/events/cliff
/mobile_base/events/digital_input
/mobile_base/events/power_system
/mobile_base/events/robot_state
/mobile_base/events/wheel_drop
/mobile_base/sensors/bumper_pointcloud
/mobile_base/sensors/core
/mobile_base/sensors/dock_ir
/mobile_base/sensors/imu_data
/mobile_base/sensors/imu_data_raw
/mobile_base/version_info
/mobile_base_nodelet_manager/bond
/move_base/DWAPlannerROS/cost_cloud
/move_base/DWAPlannerROS/global_plan
/move_base/DWAPlannerROS/local_plan
/move_base/DWAPlannerROS/parameter_descriptions
/move_base/DWAPlannerROS/parameter_updates
/move_base/DWAPlannerROS/trajectory_cloud
/move_base/NavfnROS/plan
/move_base/TrajectoryPlannerROS/global_plan
/move_base/TrajectoryPlannerROS/local_plan
/move_base/cancel
/move_base/current_goal
/move_base/feedback
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint
/move_base/global_costmap/inflation_layer/parameter_descriptions
/move_base/global_costmap/inflation_layer/parameter_updates
/move_base/global_costmap/obstacle_layer/clearing_endpoints
/move_base/global_costmap/obstacle_layer/parameter_descriptions
/move_base/global_costmap/obstacle_layer/parameter_updates
/move_base/global_costmap/parameter_descriptions
/move_base/global_costmap/parameter_updates
/move_base/global_costmap/static_layer/parameter_descriptions
/move_base/global_costmap/static_layer/parameter_updates
/move_base/goal
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint
/move_base/local_costmap/inflation_layer/parameter_descriptions
/move_base/local_costmap/inflation_layer/parameter_updates
/move_base/local_costmap/obstacle_layer/clearing_endpoints
/move_base/local_costmap/obstacle_layer/parameter_descriptions
/move_base/local_costmap/obstacle_layer/parameter_updates
/move_base/local_costmap/parameter_descriptions
/move_base/local_costmap/parameter_updates
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal
/navigation_velocity_smoother/parameter_descriptions
/navigation_velocity_smoother/parameter_updates
/navigation_velocity_smoother/raw_cmd_vel
/odom
/particlecloud
/rosout
/rosout_agg
/rtabmap/cloud_ground
/rtabmap/cloud_map
/rtabmap/cloud_obstacles
/rtabmap/global_path
/rtabmap/global_pose
/rtabmap/goal
/rtabmap/goal_node
/rtabmap/goal_out
/rtabmap/goal_reached
/rtabmap/info
/rtabmap/labels
/rtabmap/local_path
/rtabmap/mapData
/rtabmap/mapGraph
/rtabmap/mapPath
/rtabmap/move_base/cancel
/rtabmap/move_base/feedback
/rtabmap/move_base/goal
/rtabmap/move_base/result
/rtabmap/move_base/status
/rtabmap/octomap_binary
/rtabmap/octomap_empty_space
/rtabmap/octomap_full
/rtabmap/octomap_grid
/rtabmap/octomap_occupied_space
/rtabmap/odom
/rtabmap/odom_info
/rtabmap/odom_last_frame
/rtabmap/odom_local_map
/rtabmap/odom_local_scan_map
/rtabmap/proj_map
/rtabmap/scan_map
/rtabmap/user_data_async
/scan
/tf
/tf_static
And this is the what /rtabmap/odom
publishes;
We found that in the /rtabmap/odom
there are twist values.
---
header:
seq: 72
stamp:
secs: 1524337372
nsecs: 573148568
frame_id: "odom"
child_frame_id: "base_footprint"
pose:
pose:
position:
x: -0.000706656835973
y: 0.000257389328908
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -7.87830356159e-05
w: 0.999999996897
covariance: [0.00014802865635792843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00014802865635792843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00014802865635792843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00014802865635792843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00014802865635792843, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00014802865635792843]
twist:
twist:
linear:
x: -0.00201201811433
y: -0.00219572358765
z: 0.0
angular:
x: 0.0
y: -0.0
z: 0.000557422637939
covariance: [7.401432817896421e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.401432817896421e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.401432817896421e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.401432817896421e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.401432817896421e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.401432817896421e-05]
---
header:
seq: 73
stamp:
secs: 1524337372
nsecs: 956493552
frame_id: "odom"
child_frame_id: "base_footprint"
pose:
pose:
position:
x: -0.000831102253869
y: 0.00148003199138
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.00028745452235
w: 0.999999958685
covariance: [0.00015556649828067748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00015556649828067748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00015556649828067748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00015556649828067748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00015556649828067748, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00015556649828067748]
twist:
twist:
linear:
x: -0.000325132685248
y: 0.00318935327232
z: 0.0
angular:
x: 0.0
y: -0.0
z: -0.00108868733514
covariance: [7.778324914033874e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.778324914033874e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.778324914033874e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.778324914033874e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.778324914033874e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.778324914033874e-05]
---
However, we got the same values for the twist even after we set a goal, and the values are always so small that are almost zero. So we suspect this might not be the right topic for the velocity command for the motors.
Do you know which topic to subscribe to and Could you propose a solution to us?
Thank you for your time and patience.
/mobile_base/commands/velocity is the twist sent by move_base:
$ rostopic type /mobile_base/commands/velocity
geometry_msgs/Twist
Hi, Thank you guys for all the help. i also able to fix lots of issues from this thread. @rui-cao can you upload latest launch files. Im still having an error. I'm using cuda, not openGL
Hi I'm having this error now. I did All the changes mentioned here.
Can you show the TF tree?
$ rosrun tf view_frames
$ evince frames.pdf
It seems there is a missing static_transform_publisher between base_footprint and kinect frame.
Cheers, Mathieu
Hi, thank you very much for respond. Here is my launch files and frames.pdf frames.pdf my.tar.gz
PARAMETERS
NODES /rtabmap/ rgbd_odometry (rtabmap_ros/rgbd_odometry) rtabmap (rtabmap_ros/rtabmap) / base_link (tf/static_transform_publisher) depthimage_to_laserscan (nodelet/nodelet) kinect2 (nodelet/nodelet) kinect2_bridge (nodelet/nodelet) kinect2_points_xyzrgb_hd (nodelet/nodelet) kinect2_points_xyzrgb_qhd (nodelet/nodelet) kinect2_points_xyzrgb_sd (nodelet/nodelet) kobuki_safety_controller (nodelet/nodelet) move_base (move_base/move_base) navigation_velocity_smoother (nodelet/nodelet)
You don't have base_footprint
frame. Set base_link
instead as frame_id
in the demo_turtlebot_mapping.launch file. For the depthimage_to_laserscan
error, you may ask on ROS answers.
cheers, Mathieu
Hi, rui-cao @rui-cao, I met the same problem. Can you share your final launch file for the community? Many thanks. For convenience, I open an issue Autonomous Navigation with Turtlebot with Kinect2 on the autonomous task with RTAB-Map using Kinect 2. @rui-cao @matlabbe
@matlabbe Sorry to interrupt you but I am new to ROS and got into a big problem. I am running the rtabmap_ros package and trying to add the "depthimage_to_laserscan" node, the published topic /kinect_scan could be shown in rqt_graph and everthing seems ok, but the /kinect_scan of message type sensor_msgs/LaserScan has all nan values in ranges[] and empty intensities[], can you help me?
@matlabbe And here is my rtabmap.launch file.
but the /kinect_scan of message type sensor_msgs/LaserScan has all nan values in ranges[] and empty intensities[]
Are there obstacles in front of the camera? Can you post a screenshot of the depth image?
Hi,
I am following your tutorial on Mapping and Navigation with Turtlebot.
I have problem to make the tutrtlebot to work with the Kinect2. The reason we found out is that turtlebot does not have 3d_sensor launch file to Kinect2 driver (Freenect2 and iai_kinect/kinect2_bridge). I tried to modify the original Freenect (driver to the older Kinect) launch file which is already in the turtlebot, to make it compatible with the Kinect2, by pointing to kinect2_bridge.launch, and modified the arguments to make it compatible. However it does not work. If you are interested, more detail can be viewed from my wiki page.
While not using the turtlebot package, your package was working perfectly with Kinect2 on my computer, both in standalone and the ros version. I am using ROS Kinetic.
Thank you for your time!