How to get a custom fixed frame tf with hector_slam?
Hello!
I'm currently using the slam branch of the rplidarros package which uses Hectorslam in order to localize a mecanum wheel robot in a room. Since the robot will only need to be inside this room, I would like to fix a reference frame in the middle of the room, but I fail to see how to use the tf package to do so.
I tried creating a fixed frame broadcaster with the wanted origin frame fixed_map as parent of the usual base_link frame given by the hectorposeestimation node:
#!/usr/bin/env python
# import roslib
import rospy
import tf
import time
from time import sleep
import math
from geometry_msgs.msg import Pose2D
if __name__ == '__main__':
rospy.init_node('fixed_tf_broadcaster')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
br.sendTransform((0.0, 2.0, 0.0),
(0.0, 0.0, 0.0, 1.0),
rospy.Time.now(),
"base_link",
"fixed_map")
rate.sleep()
and modifing the given launch by commenting the link1_broadcaster, creating a new broadcaster called origin and changing the base_frame parameter by the fixed_frame I believe I'm broadcasting:
<launch>
<!-- <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 base_link laser 100" /> -->
<node pkg="bot" type="map_tf.py" name="origin" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping" output="screen">
<param name="scan_topic" value="scan" />
<!-- <param name="base_frame" value="base_link" /> -->
<param name="fixed_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<param name="output_timing" value="false"/>
<param name="advertise_map_service" value="true"/>
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<param name="pub_map_odom_transform" value="true"/>
<param name="map_with_known_poses" value="false"/>
<param name="map_pub_period" value="0.5"/>
<param name="update_factor_free" value="0.45"/>
<param name="laser_max_dist" value="3.7"/>
<param name="map_update_distance_thresh" value="0.02"/>
<param name="map_update_angle_thresh" value="0.1"/>
<param name="map_resolution" value="0.05"/>
<param name="map_size" value="1024"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5"/>
</node>
</launch>
The result is I can launch the launch file wihtout problem:
SUMMARY
========
PARAMETERS
* /hector_height_mapping/advertise_map_service: True
* /hector_height_mapping/fixed_frame: base_link
* /hector_height_mapping/laser_max_dist: 3.7
* /hector_height_mapping/map_pub_period: 0.5
* /hector_height_mapping/map_resolution: 0.05
* /hector_height_mapping/map_size: 1024
* /hector_height_mapping/map_start_x: 0.5
* /hector_height_mapping/map_start_y: 0.5
* /hector_height_mapping/map_update_angle_thresh: 0.1
* /hector_height_mapping/map_update_distance_thresh: 0.02
* /hector_height_mapping/map_with_known_poses: False
* /hector_height_mapping/odom_frame: base_link
* /hector_height_mapping/output_timing: False
* /hector_height_mapping/pub_map_odom_transform: True
* /hector_height_mapping/scan_topic: scan
* /hector_height_mapping/update_factor_free: 0.45
* /hector_height_mapping/use_tf_pose_start_estimate: False
* /hector_height_mapping/use_tf_scan_transformation: True
* /rosdistro: kinetic
* /rosversion: 1.12.13
* /rplidarNode/angle_compensate: True
* /rplidarNode/frame_id: laser
* /rplidarNode/inverted: False
* /rplidarNode/serial_baudrate: 115200
* /rplidarNode/serial_port: /dev/ttyUSB0
NODES
/
hector_height_mapping (hector_mapping/hector_mapping)
origin (bot/map_tf.py)
rplidarNode (rplidar_ros/rplidarNode)
rviz (rviz/rviz)
auto-starting new master
process[master]: started with pid [3183]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 5cc94820-2468-11e8-ae56-b827ebc52417
process[rosout-1]: started with pid [3196]
started core service [/rosout]
process[rplidarNode-2]: started with pid [3213]
process[origin-3]: started with pid [3214]
process[hector_height_mapping-4]: started with pid [3223]
process[rviz-5]: started with pid [3233]
RPLIDAR running on ROS package rplidar_ros
SDK Version: 1.5.7
HectorSM map lvl 0: cellLength: 0.05 res x:1024 res y: 1024
HectorSM map lvl 1: cellLength: 0.1 res x:512 res y: 512
HectorSM map lvl 2: cellLength: 0.2 res x:256 res y: 256
[ INFO] [1520689135.328928743]: HectorSM p_base_frame_: base_link
[ INFO] [1520689135.329487337]: HectorSM p_map_frame_: map
[ INFO] [1520689135.329763014]: HectorSM p_odom_frame_: base_link
[ INFO] [1520689135.329990201]: HectorSM p_scan_topic_: scan
[ INFO] [1520689135.330214785]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1520689135.330413170]: HectorSM p_pub_map_odom_transform_: true
[ INFO] [1520689135.330544785]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1520689135.330684785]: HectorSM p_map_pub_period_: 0.500000
[ INFO] [1520689135.330814056]: HectorSM p_update_factor_free_: 0.450000
[ INFO] [1520689135.330958066]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1520689135.331090358]: HectorSM p_map_update_distance_threshold_: 0.020000
[ INFO] [1520689135.331232024]: HectorSM p_map_update_angle_threshold_: 0.100000
[ INFO] [1520689135.331365514]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1520689135.331501139]: HectorSM p_laser_z_max_value_: 1.000000
failed to get the current screen resources
RPLIDAR S/N: C5F5FBF8C5E299F1C4E592F734205B3F
Firmware Ver: 1.20
Hardware Rev: 0
RPLidar health status : 0
QXcbConnection: XCB error: 170 (Unknown), sequence: 163, resource id: 90, major code: 146 (Unknown), minor code: 20
But once the rviz terminal opens, there is an error: Fixed Frame [map] does not exist and therefore I can't get the /scan topic nor the map. While echoing the /tf topic I get the transformation I imposed:
---
transforms:
-
header:
seq: 0
stamp:
secs: 1520688972
nsecs: 640590906
frame_id: "fixed_map"
child_frame_id: "base_link"
transform:
translation:
x: 0.0
y: 2.0
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
So, I was wondering what's the correct way to set up a custom fixed frame in a map, which doesn't correspond to the initial pose of the robot.
I'm running ROS Kinetic on a ubuntu mate 16.04.4 LTS on a raspberry pi 3.
Thank you!
Asked by kharkad on 2018-04-10 04:44:17 UTC
Comments
Have you tried setting the fixed frame in rviz to 'fixed_map' instead of 'map'? Also I'm not familiar with hector but it seems like the param
fixed_frame
should be a world-fixed frame like yourfixed_map
frame and not the framebase_link
which is attached to the robot.Asked by achille on 2018-08-02 12:41:43 UTC