0
$\begingroup$

Rosanswers logo

Hi,

I'm trying to use hector mapping on my custom robot to produce a map, this works fine without any odometry information from my odom node.

here's the launch file (i didn't write it, but it works pretty well).

<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"    output="screen">

<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<!-- Map size / start point -->
<param name="map_resolution" value="0.025"/>
<param name="map_size" value="2048"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value="-2.5" />
<param name="laser_z_max_value" value="7.5" />

<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.7" />    
<param name="map_update_distance_thresh" value="0.2"/>
<param name="map_update_angle_thresh" value="0.06" />
</node>

<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster"      args="0 0 0 0 0 0 /base_link /laser 100" />  

</launch>

The problem is when i try and use it with my odometry information, published from my odom node (which has the same framework as the one from the odom tutorial, but is customised to take my encoder data and use my equations) -

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float32.h> 
#include <math.h>

#define RovWid 0.25
#define Pi 3.141592865358979
#define TicksPerRev 600
#define WheelDia 0.13

float left,right; //declare global variables to hold incoming data

void CallbackLeft(const std_msgs::Float32::ConstPtr& msg)
{
  //ROS_INFO("Left ticks [%f]", msg->data);
  left = msg->data;
}

void CallbackRight(const std_msgs::Float32::ConstPtr& msg)
{
  //ROS_INFO("Right ticks [%f]", msg->data);
  right = msg->data;
}

int main(int argc, char** argv){
    ros::init(argc, argv, "odometry_publisher");

    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    ros::Subscriber sub1 = n.subscribe("ticks_left", 100, CallbackLeft);
    ros::Subscriber sub2 = n.subscribe("ticks_right", 100, CallbackRight);
    tf::TransformBroadcaster odom_broadcaster;

    double vx = 0.0;
    double vy = 0.0;
    double vth = 0.0;

    float DeltaLeft = 0;
    float DeltaRight = 0;
    float PreviousRight = 0;
    float PreviousLeft = 0;

    float Theta = 0;
    float X = 0;
    float Y = 0;

    float Circum, DisPerTick, expr1,right_minus_left;

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    ros::Rate r(5);
    ROS_INFO("Node started");
    Circum = Pi * WheelDia;
    DisPerTick = Circum / TicksPerRev;

    while(n.ok()){

          ros::spinOnce();               // check for incoming messages
          current_time = ros::Time::now();

          DeltaRight = (right - PreviousRight) * DisPerTick;
          DeltaLeft = (left - PreviousLeft) * DisPerTick;
          PreviousRight = right;
          PreviousLeft = left;


          if (DeltaLeft == DeltaRight){
                X += DeltaLeft * cos(Theta);
                Y += DeltaLeft * sin(Theta);
          }
          else{
                 expr1 = RovWid * 2 * (DeltaRight + DeltaLeft)/ 2.0 / (DeltaRight - DeltaLeft);
                 right_minus_left = DeltaRight - DeltaLeft;
                 X += expr1 * (sin(right_minus_left / (RovWid *2) + Theta) - sin(Theta));
                 Y -= expr1 * (cos(right_minus_left / (RovWid *2) + Theta) - cos(Theta));
                 Theta += right_minus_left / (RovWid *2);
                 Theta = Theta - ((2 * Pi) * floor( Theta / (2 * Pi)));
          }

          ROS_INFO("X [%f]", X);
          ROS_INFO("Y [%f]", Y);
          ROS_INFO("Theta [%f]", Theta);
          ROS_INFO("\n");

          geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Theta);

          geometry_msgs::TransformStamped odom_trans;
          odom_trans.header.stamp = current_time;
          odom_trans.header.frame_id = "odom";
          odom_trans.child_frame_id = "base_link";

          odom_trans.transform.translation.x = X;
          odom_trans.transform.translation.y = Y;
          odom_trans.transform.translation.z = 0.0;
          odom_trans.transform.rotation = odom_quat;

          odom_broadcaster.sendTransform(odom_trans);

          nav_msgs::Odometry odom;
          odom.header.stamp = current_time;
          odom.header.frame_id = "odom";

          odom.pose.pose.position.x = X;
          odom.pose.pose.position.y = Y;
          odom.pose.pose.position.z = 0.0;
          odom.pose.pose.orientation = odom_quat;

          odom.child_frame_id = "base_link";
          odom.twist.twist.linear.x = 0;
          odom.twist.twist.linear.y = 0;
          odom.twist.twist.angular.z = 0;

          odom_pub.publish(odom);

          last_time = current_time;
          r.sleep();
         }

}

This published odometry information just fine. The problem is when i try and incorporate the two.

Following the hector slam robot setup, i change the base frame and odom frame too:

<param name="base_frame" value="base_frame" />
<param name="odom_frame" value="odom" />

but i get the following timeout:

[ INFO] [1490208346.381940938]: lookupTransform base_frame to laser timed out. Could not transform laser scan into base_frame.

This stops the slam node from creating anything

When i change the base frame to base_link as it was in the original, i get the following error:

[ERROR] [1490208025.858563554]: Transform failed during publishing of map_odom transform: Lookup would require extrapolation at time 1490208024.946150226, but only time 1490208024.845273926 is in the buffer, when looking up transform from frame [base_link] to frame [odom]

This error still allows for mapping, but makes no use of my odometry info.

No doubt the problem is me not setting up the required transfrom, but im genuinely at a loss with what to do about it. Help would be greatly appreciated. At the very least i hope someone finds my code useful.


Originally posted by Ben12345 on ROS Answers with karma: 98 on 2017-03-22

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I think the problem is this. In your launch file you write:

<param name="odom_frame" value="base_link" />

But in your node you write the following:

ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

So, i think, you may change code line into next view in your launch file:

 <param name="odom_frame" value="odom" />

Originally posted by Edward with karma: 76 on 2017-08-01

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.