not getting map in gmapping transform from odom to base_link problem

asked 2017-12-09 11:08:05 -0500

Shanky13 gravatar image

updated 2017-12-10 05:16:23 -0500

gvdhoorn gravatar image

i m using gmapping and i m not able to transform from odom to base_link. i have the odometry on wheel_odometry topic which has header frame id as odom and child frame id as base_link further i m using static transform from base_link to laser. i wrote a code to subscribe to wheel_odometry topic and transform it from to odom to base_link and publish it on odomerty topic but still not able to get the map i m sending the code below when i run the code either i get segmentation failed code dumped or else i dont get the transform i m getting the transform in tf tree as map->odom->base_link->laser when the segmentation core is not dumped also i get the error in gmapping as:

[ WARN] [1512827700.862662060, 1509362043.864097101]: MessageFilter [target=odom ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_notifier] rosconsole logger to DEBUG for more information.

so what is the problem and and how to correct it

#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include <tf/transform_broadcaster.h>

void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg) {
  ros::NodeHandle nh;
  ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
  ros::Rate r(1.0);
  while (nh.ok()) {
    ros::spinOnce();
    ros::Time current_time, last_time;
    tf::TransformBroadcaster odom_broadcaster;
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;
    double z = msg->pose.pose.position.z;
    double a = msg->pose.pose.orientation.x;
    double b = msg->pose.pose.orientation.y;
    double c = msg->pose.pose.orientation.z;
    double d = msg->pose.pose.orientation.w;

    double vx = msg->twist.twist.linear.x;
    double vy = msg->twist.twist.linear.y;
    double vz = msg->twist.twist.angular.z;
    current_time = ros::Time::now();
    double dt = (ros::Time::now() - last_time).toSec();
    double delta_x = (vx * cos(z) - vy * sin(z)) * dt;
    double delta_y = (vx * sin(z) + vy * cos(z)) * dt;
    double delta_z = vz * dt;

    x += delta_x;
    y += delta_y;
    z += delta_z;
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(z);

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros::Time::now();
    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 = z;
    odom_trans.transform.rotation.x = a;
    odom_trans.transform.rotation.y = b;
    odom_trans.transform.rotation.z = c;
    odom_trans.transform.rotation.w = d;

    odom_broadcaster.sendTransform(odom_trans);
    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";

    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = z;
    odom.pose.pose.orientation.x = a;
    odom.pose.pose.orientation.y = b;
    odom.pose.pose.orientation.z = c;
    odom.pose.pose.orientation.w = d;

    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vz;

    last_time = ros::Time::now();
    odom_pub.publish(odom);
    r.sleep();
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "odom_listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("wheel_odometry", 100, chatterCallback);
  ros::spin();
  return 0;
}
edit retag flag offensive close merge delete

Comments

The behaviour clearly Points to the transform tree not being properly set up. I.e. the tranform from odom->base_link seems to be not correct. However, without more Information, we cannot debug this.

Try if you can rosrun tf tf_monitor odom base_link continuously with low delay.

mgruhler gravatar image mgruhler  ( 2017-12-11 01:36:45 -0500 )edit

looking at your code, there are several things wrong. You should not have the while Loop in the callback. This is handled by the ros::spin() call. Also, you should not instantiate the transform_broadcaster and the Publisher in the callback. Those could be the Errors here.

mgruhler gravatar image mgruhler  ( 2017-12-11 01:38:49 -0500 )edit