querry with costmap2d at negative zaxis
I am trying to create a cost-map . My robot is a underwater vehicle so the coordinates have negative z axis. The issue is that cost-map2d doesn't form at x,y,0 not at negative z. I am using UUV simulator
My files for the program are
costmap.launch
<launch>
<!--node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1.0 odom world 100" /-->
<node pkg="tf" type="static_transform_publisher" name="link2_broadcaster" args="0 0 0 0 0 0 1.0 map world 100" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="/home/cair/heauv/heauv_sim/src/heauv_costmap/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="/home/cair/heauv/heauv_sim/src/heauv_costmap/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="/home/cair/heauv/heauv_sim/src/heauv_costmap/global_costmap_params.yaml" command="load" />
<rosparam file="/home/cair/heauv/heauv_sim/src/heauv_costmap/local_costmap_params.yaml" command="load" />
</node>
</launch>
costmap_common_params.yaml
obstacle_range: 10.0
raytrace_range: 10.0
footprint: [[0,0], [2, 0],[0, 0.25],[2,0.25]]
global_frame: map
robot_base_frame: eca_a9/base_link
robot_radius: 0.05
#inflation_radius: 0.55
transform_tolerance: 1
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: eca_a9/sonarfront_link, data_type: LaserScan, topic: /eca_a9/sss_front, marking: true, clearing: true}
#point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}
global_costmap_params.yaml
global_costmap:
update_frequency: 5.0
static_map: false
origin_x: 0.0
origin_y: 0.0
transform_tolerance: 1.0
rolling_window: true
width: 100.0
height: 100.0
resolution: 0.5
min_obstacle_height: -100
plugins:
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
local_costmap_params.yaml
local_costmap:
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 10.0
height: 10.0
resolution: 0.05
transform_tolerance: 1.0
plugins:
- {name: inflation_layer, type: "costmap_2d::InflationLayer"} - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
Here is the snapshort of the issue
As you can see the the pic. The robot is at negative z but costmap is formed at z=0
some topic to help understand better.
rosrun tf tf_echo map eca_a9/base_link. output
At time 45.334
- Translation: [-0.001, -0.000, -4.323]
- Rotation: in Quaternion [0.000, 0.001, -0.000, 1.000]
in RPY (radian) [0.000, 0.003, -0.000]
rostopic echo /move_base/local_costmap/footprint outputs
header:
seq: 980
stamp:
secs: 225
nsecs: 212000000
frame_id: "map"
polygon:
points:
-
x: -49.4810142517
y: 2.74199464911e-05
z: 0.0
-
x: -47.4710159302
y: 0.000102666992461
z: 0.0
-
x: -49.4810256958
y: 0.2600274086
z: 0.0
-
x: -47.4710235596
y: 0.260102659464
z: 0.0
Here z is stuck at 0.0 .
rostopic echo /move_base/local_costmap/costmap
stmap
header:
seq: 0
stamp:
secs: 336
nsecs: 92000000
frame_id: "map"
info:
map_load_time:
secs: 0
nsecs: 0
resolution: 0.0500000007451
width: 200
height: 200
origin:
position:
x: -43.55
y: -4.95
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ...