no range_sensor_layer on local costmap

asked 2019-08-16 08:08:03 -0500

june2473 gravatar image

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

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 imagejune2473 ( 2019-08-19 02:04:19 -0500 )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 imageHumpelstilzchen ( 2019-08-20 11:04:01 -0500 )edit