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
void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg) {
ros::NodeHandle nh;
ros::Publisher odom_pub = nh.advertise("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;
}
↧