How to get a custom fixed frame tf with hector_slam?

asked 2018-04-10 04:44:17 -0500

kharkad gravatar image

updated 2018-04-10 10:00:32 -0500

Hello!

I'm currently using the slam branch of the rplidar_ros package which uses Hector_slam 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 hector_pose_estimation 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 ...
(more)
edit retag flag offensive close merge delete

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 your fixed_map frame and not the frame base_link which is attached to the robot.

achille gravatar image achille  ( 2018-08-02 12:41:43 -0500 )edit