no range_sensor_layer on local costmap
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 ...
upd. Now, I found out that i can use static_transform_publisher instead of my own script tf_map.py
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?