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).
#include
#include
#include
#include
#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("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:
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.
↧