no range_sensor_layer on local costmap

asked 2019-08-16 08:08:03 -0600

june2473 gravatar image

updated 2019-08-16 08:39:18 -0600

i am trying to visualize range_sensor_layer on local_costmap in rviz,

but i see nothing, just white square

  move_base.launch:
    <launch>
    <!-- Arguments -->

    <arg name="cmd_vel_topic" default="/cmd_vel" />
    <arg name="odom_topic" default="/camera/odom/sample" />

    <!-- move_base -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

      <param name="base_global_planner" value="navfn/NavfnROS" />
      <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
      <rosparam file="$(find range_sensor_reader)/param/move_base_params.yaml" command="load" />

      <rosparam file="$(find range_sensor_reader)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find range_sensor_reader)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />

      <rosparam file="$(find range_sensor_reader)/param/local_costmap_params.yaml" command="load" />

      <rosparam file="$(find range_sensor_reader)/param/global_costmap_params.yaml" command="load" />

      <rosparam file="$(find range_sensor_reader)/param/dwa_local_planner_params.yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
    </node>
  </launch>

costmap_common_params.yaml:

obstacle_range: 60
raytrace_range: 50
footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17
inflation_radius: 1.0
cost_scaling_factor: 3.0
map_type: costmap

global_costmap_params.yaml:

global_costmap:
  global_frame: map
  robot_base_frame: camera_link

  update_frequency: 1.0
  publish_frequency: 1.0
  transform_tolerance: 0.2

  static_map: true

  plugins:
    - {name: static,            type: "costmap_2d::StaticLayer"}
    - {name: inflation,         type: "costmap_2d::InflationLayer"}
    - {name: sonar,             type: "range_sensor_layer::RangeSensorLayer"}

  static:
    map_topic: /map
    subscribe_to_updates: true

  inflation:
    inflation_radius: 0.4

  sonar:
    frame: range_sensor_frame
    topics: ["/sensor_us","sensor_ir"]
    no_readings_timeout: 2.0
    clear_on_max_reading: false

local_costmap_params.yaml:

local_costmap:
  global_frame: camera_odom_frame
  robot_base_frame: camera_link

  update_frequency: 1.0
  publish_frequency: 1.0
  transform_tolerance: 0.2

  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

  plugins:
    - {name: sonar,             type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation,         type: "costmap_2d::InflationLayer"}

  inflation:
    inflation_radius: 0.4

  sonar:
    frame: range_sensor_frame
    topics: ["/sensor_us","sensor_ir"]
    no_readings_timeout: 2.0
    clear_on_max_reading: false

move_base_params.yaml:

shutdown_costmaps: false
controller_frequency: 10.0
NavfnROS:
    allow_unknown: true
    default_tolerance: 0.1
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 5.0
oscillation_distance: 0.20

my script that i use to setup frames, tf_broadcaster.py:

#!/usr/bin/env python  
import rospy

# Because of transformations
import tf_conversions

import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg
import sensor_msgs.msg

def handle_camera_pose(msg):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "camera_link"
    t.child_frame_id = "range_sensor_frame"
    t.transform.translation.x = 0
    t.transform.translation.y = 0.02
    t.transform.translation.z = 0.04
    q = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

    br.sendTransform(t)

if __name__ == '__main__':
    rospy.init_node('range_sensor_frame_broadcaster')
    rospy.Subscriber('/sensor_us', sensor_msgs.msg.Range, handle_camera_pose)
    rospy.spin()

another script for map and camera_link, tf_map.py:

#!/usr/bin/env python  
import rospy

# Because of transformations
import tf_conversions
import tf

import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg
import nav_msgs.msg

if __name__ == '__main__':
    rospy.init_node('map_camera_tf_broadcaster')

    broadcaster = tf2_ros.StaticTransformBroadcaster()
    static_transformStamped = geometry_msgs.msg.TransformStamped()

    static_transformStamped.header.stamp = rospy.Time.now()
    static_transformStamped.header.frame_id = "map"
        static_transformStamped.child_frame_id = "camera_odom_frame"

        static_transformStamped ...
(more)
edit retag flag offensive close merge delete

Comments

upd. Now, I found out that i can use static_transform_publisher instead of my own script tf_map.py

    <node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /camera_odom_frame 100"/>
june2473 gravatar image june2473  ( 2019-08-19 02:04:19 -0600 )edit

A static transform from /map to /camera_odom_frame sounds wrong for me. This transform is supposed to be calculated by e.g. gmapping at runtime. With a static transform your robot can not move in reference to the map frame. Could it be that you originally wanted to change the fixed frame in rviz?

Humpelstilzchen gravatar image Humpelstilzchen  ( 2019-08-20 11:04:01 -0600 )edit