Problem publishing from txt file to topic
Hi
I have a txt file with encoder data that looks like this:
...
1 277.87333332 269.94188565 2111.27302100 2111.27305200 2.40910042 0.58907051
1 277.88407118 269.94457012 2111.27810100 2111.27812500 2.33276239 0.58943359
1 277.89595953 269.94725459 2111.28316700 2111.28318600 2.33464865 0.57762184
1 277.90746439 269.94993905 2111.28825800 2111.28828900 2.32527232 0.56519989
1 277.91935274 269.95262352 2111.29334100 2111.29336900 2.30780781 0.54064191
1 277.93315857 269.95569148 2111.29842600 2111.29847000 2.36224113 0.54073054
1 277.94658090 269.95875944 2111.30351200 2111.30353700 2.38172210 0.55344243
1 277.96153721 269.96182740 2111.30858400 2111.30860500 2.42912526 0.56616211
1 277.97572653 269.96451187 2111.31365500 2111.31367500 2.46379925 0.56604012
1 277.98953236 269.96796333 2111.31872300 2111.31873900 2.50533659 0.59181053
...
I want to publish this data to an odometry publisher. In the original program a while loop is used for publishing the topics. I had to remove this loop to come back into my for loop, but now the /odom and /tf topic are not constantly published. What can I do to solve this?
Afterwards I want to record the /tf topic, together with a sensordata topic (with the same timestamps). But the publishing of these topics is not equal with the timestamps, does de rosbag arrange the topics with help from the timestamps I set in the publisher (coming from the txt file).
This is my code: int main(int argc, char **argv) {
ros::init(argc, argv, "odometry_from_text_publisher");
ros::NodeHandle n;
ros::Rate r(100.0);
//ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
//tf::TransformBroadcaster odom_broadcaster;
std::ifstream file("/home/bert/catkin_ws/src/mastering_ros_demo_pkg/src/WheelEncoders1.txt");
std::vector<int> data;
if(file.is_open())
{
for(int i=0;i<36747 && file.good() ;i++)
{
double index;
//Created a message
//mastering_ros_demo_pkg::angle_msg msg;
file >> index;
file >> angle_left;
file >> angle_right;
file >> angle_timestamp1;
file >> angle_timestamp2;
file >> index2;
file >> index3;
//Printing message data
//ROS_INFO("%f",index);
ROS_INFO("%f",angle_left);
ROS_INFO("%f",angle_right);
ROS_INFO("%f",angle_timestamp1);
//ROS_INFO("%f",msg.angle_timestamp2);
//ROS_INFO("%f",index2);
//ROS_INFO("%f",index3);
//ros::init(argc, argv, "odometry_from_text_publisher");
//ros::NodeHandle n;
//tijd van de meting
ros::Time current_time_encoder(angle_timestamp1);
//verschil in hoek tov vorige berekening
deltaLeft = angle_left - _PreviousLeftEncoderAngle;
deltaRight = angle_right - _PreviousRightEncoderAngle;
//snelheid links (x) en rechts (y)
vx = deltaLeft * DistancePerRadian / (current_time_encoder - last_time_encoder).toSec();
vy = deltaRight * DistancePerRadian / (current_time_encoder - last_time_encoder).toSec();
//als snelheid links = snelheid rechts -> snelheid = snelheid links en hoeksnelheid = 0
if(abs(vx-vy)<0.00001)
//if (vx == vy)
{
V = vx;
W = 0;
}
else
{
// Assuming the robot is rotating about point A
// W = vel_left/r = vel_right/(r + d), see the image below for r and d
double r = (vx * d) / (vy - vx); // Anti Clockwise is positive
W = vx/r; // Rotational velocity of the robot
V = W * (r + d/2); // Translation velocity of the robot
}
//vth = hoeksnelheid
vth= W;
//hoek onthouden
_PreviousLeftEncoderAngle = angle_left;
_PreviousRightEncoderAngle = angle_right;
//tijd van de laatste meting onthouden
last_time_encoder = current_time_encoder;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
//ros::Time current_time, last_time;
//current_time = ros::Time::now();
//last_time = ros::Time::now();
ros::Time current_time(angle_timestamp1);
ros::Time last_time(angle_timestamp1);
//ros::Rate r(100.0);
//while(n.ok())
//{
//current_time = ros::Time::now();
//ros::Time current_time(angle_timestamp1);
//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_th = vth * dt;
th += delta_th;
double delta_x = V * cos(th) * dt;
double delta_y = V * sin(th) * dt;
// double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
// double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
x += delta_x;
y += delta_y;
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
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;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position (actuele positie van de robot)
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;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = V; //snelheid in de x-richting
odom.twist.twist.linear.y = 0; //snelheid in de y-richting
odom.twist.twist.angular.z =W; // hoeksnelheid robot
//publish the message
odom_pub.publish(odom);
last_time = current_time;
/*
//ros::spinOnce();
//r.sleep();
//}
*/
}
ROS_INFO("End of file");
}
else
{
ROS_INFO("file not found!");
ros::shutdown();
}
}
Thanks!
Asked by Bert on 2016-04-28 04:12:58 UTC
Comments