Posted by
gian on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Comparison-between-realsense-D435-vs-T265-vs-T265-D435-dual-setup-tp6456p6955.html
Hello Mathieu,
I'm trying to adapt the nav3d launch file for the configuration d435+t265, but move_base doesn't receive the goal sent by rviz.
After the mapping , I am launching rtabmap in localization mode from command line as in you Handheld Mapping tutorial :
roslaunch rtabmap_ros rtabmap.launch args:=" --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3 " odom_topic:=/t265/odom/sample frame_id:=t265_link rgbd_sync:=true depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/d400/color/camera_info approx_rgbd_sync:=false visual_odometry:=false localization:=true
After I start the azimuth nav3 launch file modified by me for the d435 , only for navigation .I see the obastacles/ground cloud but robot doesn't move ( cmd_vel doesn't receive nothing)
<launch>
<arg name="cmd_vel_topic" default="/cmd_vel" />
<arg name="move_forward_only" default="false"/>
<arg name="odom_topic" default="/t265/odom/sample"/>
<!-- ROS navigation stack move_base -->
<group ns="planner">
<remap from="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/rtabmap/grid_map"/>
<remap from="cmd_vel" to="/cmd_vel"/>
<remap from="/move_base_simple/goal" to="/rtabmap/goal_out"/>
<remap from="odom" to="/t265/odom/sample"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
</group>
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />
<node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz nodelet_manager">
<remap from="depth/image" to="/d400/aligned_depth_to_color/image_raw"/>
<remap from="depth/camera_info" to="/d400/color/camera_info"/>
<remap from="cloud" to="cloudXYZ" />
<param name="decimation" type="int" value="4"/>
<param name="max_depth" type="double" value="3.0"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/obstacles_cloud"/>
<remap from="ground" to="/ground_cloud"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
</node>
</group>
</launch>
I left the costmap files almost as original ones, I modified only the odom frame ( t265_odom_frame) in the local costmap ,
and sensor_frame: d400_link and topic: /camera/cloudXYZ in the local_costmap_params_3d.yaml file
local_costmap_params.yaml
costmap_params.yaml
global_frame: t265_odom_frame
robot_base_frame: base_footprint
update_frequency: 2
publish_frequency: 1
rolling_window: true
width: 3.0
height: 3.0
resolution: 0.025
origin_x: 0
origin_y: 0
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
local_costmap_params_2d.yaml
costmap_params_2d.yaml
global_frame: t265_odom_frame
robot_base_frame: base_footprint
update_frequency: 2.0
publish_frequency: 2.0
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.025
origin_x: -2.0
origin_y: -2.0
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
obstacle_layer:
obstacle_range: 2.5
raytrace_range: 3.0
max_obstacle_height: 0.4
track_unknown_space: true
observation_sources: point_cloud_sensorA point_cloud_sensorB
point_cloud_sensorA: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: obstacles_cloud,
expected_update_rate: 0.5,
marking: true,
clearing: true,
min_obstacle_height: 0.04
}
point_cloud_sensorB: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: ground_cloud,
expected_update_rate: 0.5,
marking: false,
clearing: true,
min_obstacle_height: -1.0 # make usre the ground is not filtered
}
local_costmap_params_3d.yaml
costmap_params_3d.yaml
local_costmap:
global_frame: t265_odom_frame
robot_base_frame: base_footprint
update_frequency: 2.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.025
origin_x: -2.0
origin_y: -2.0
#observation_sources: laser_scan_sensor point_cloud_sensor
observation_sources: point_cloud_sensor
# assuming receiving a cloud from rtabmap/obstacles_detection node
point_cloud_sensor: {
sensor_frame: d400_link
data_type: PointCloud2,
topic: /camera/cloudXYZ
expected_update_rate: 0.5,
marking: true,
clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 0.5}
Could you please advice how to correct my mistakes ?
Thanks a lot
Gian