Ask Your Question
0

hector slam launch with odom and hokuyo

asked 2017-03-22 13:49:27 -0500

Ben12345 gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-08-01 01:58:09 -0500

Edward gravatar image

updated 2017-08-01 01:58:37 -0500

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" />
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-03-22 13:49:27 -0500

Seen: 840 times

Last updated: Aug 01 '17