querry with costmap2d at negative zaxis

asked 2020-12-15 08:00:28 -0500

shashank gravatar image

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 image description

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 ...
(more)
edit retag flag offensive close merge delete