ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hello,
I started from your code and did some changes in order to use an Odometry message to measure how much the robot moved.
I couldn't format the code properly using this editor, so I created a post: http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/
Basically, you have to consider the current position of the robot to have closed loop. The way it's right now, it relays on time and a very good calibration of the robot, what is not recommended if you want to have a node as generic that could be used in different robots and environments.
Don't forget that you have to add in your CMakeLists.txt and package.xml the dependency nav_msgs before trying to compile.
2 | No.2 Revision |
Hello,
I started from your code and did some changes in order to use an Odometry message to measure how much the robot moved.
I couldn't format the code properly using this editor, so I created a post: http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/
If you want to reproduce my experiment, you can use the Turtlebot 2 public simulation available in RDS (https://rds.theconstructsim.com/). But you can do it in your own computer too, using this robot.
Basically, you have to consider the current position of the robot to have closed loop. The way it's right now, it relays on time and a very good calibration of the robot, what is not recommended if you want to have a node as generic that could be used in different robots and environments.
Don't forget that you have to add in your CMakeLists.txt and package.xml the dependency nav_msgs before trying to compile.
3 | No.3 Revision |
Hello,
I started from your code and did some changes in order to use an Odometry message to measure how much the robot moved.
I couldn't format the code properly using this editor, so I created a post:
http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/
But you can see the code below:
#include
#include
#include
#include
#include
#include
geometry_msgs::Pose2D current_pose;
ros::Publisher pub_pose2d;
void odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
// linear position
current_pose.x = msg->pose.pose.position.x;
current_pose.y = msg->pose.pose.position.y;
// quaternion to RPY conversion
tf::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// angular position
current_pose.theta = yaw;
pub_pose2d.publish(current_pose);
}
int main(int argc, char **argv)
{
const double PI = 3.14159265358979323846;
ROS_INFO("start");
ros::init(argc, argv, "move_pub");
ros::NodeHandle n;
ros::Subscriber sub_odometry = n.subscribe("odom", 1, odomCallback);
ros::Publisher movement_pub = n.advertise("cmd_vel",1); //for sensors the value after , should be higher to get a more accurate result (queued)
pub_pose2d = n.advertise("turtlebot_pose2d", 1);
ros::Rate rate(10); //the larger the value, the "smoother" , try value of 1 to see "jerk" movement
//move forward
ROS_INFO("move forward");
ros::Time start = ros::Time::now();
while(ros::ok() && current_pose.x < 1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//turn right
ROS_INFO("turn right");
ros::Time start_turn = ros::Time::now();
while(ros::ok() && current_pose.theta > -PI/2)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0; //speed value m/s
move.angular.z = -0.3;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//move forward again
ROS_INFO("move forward");
ros::Time start2 = ros::Time::now();
while(ros::ok() && current_pose.y > -1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
// just stop
while(ros::ok()) {
geometry_msgs::Twist move;
move.linear.x = 0;
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
return 0;
}
If you want to reproduce my experiment, you can use the Turtlebot 2 public simulation available in RDS (https://rds.theconstructsim.com/). But you can do it in your own computer too, using this robot.
Basically, you have to consider the current position of the robot to have closed loop. The way it's right now, it relays on time and a very good calibration of the robot, what is not recommended if you want to have a node as generic that could be used in different robots and environments.
Don't forget that you have to add in your CMakeLists.txt and package.xml the dependency nav_msgs before trying to compile.
4 | No.4 Revision |
Hello,
I started from your code and did some changes in order to use an Odometry message to measure how much the robot moved.
I couldn't format the code properly using this editor, so I created a post: http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/ But you can see the code below:
#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
geometry_msgs::Pose2D current_pose;
ros::Publisher pub_pose2d;
void odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
// linear position
current_pose.x = msg->pose.pose.position.x;
current_pose.y = msg->pose.pose.position.y;
// quaternion to RPY conversion
tf::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// angular position
current_pose.theta = yaw;
pub_pose2d.publish(current_pose);
}
int main(int argc, char **argv)
{
const double PI = 3.14159265358979323846;
ROS_INFO("start");
ros::init(argc, argv, "move_pub");
ros::NodeHandle n;
ros::Subscriber sub_odometry = n.subscribe("odom", 1, odomCallback);
ros::Publisher movement_pub = n.advertise("cmd_vel",1); //for sensors the value after , should be higher to get a more accurate result (queued)
pub_pose2d = n.advertise("turtlebot_pose2d", 1);
ros::Rate rate(10); //the larger the value, the "smoother" , try value of 1 to see "jerk" movement
//move forward
ROS_INFO("move forward");
ros::Time start = ros::Time::now();
while(ros::ok() && current_pose.x < 1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//turn right
ROS_INFO("turn right");
ros::Time start_turn = ros::Time::now();
while(ros::ok() && current_pose.theta > -PI/2)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0; //speed value m/s
move.angular.z = -0.3;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//move forward again
ROS_INFO("move forward");
ros::Time start2 = ros::Time::now();
while(ros::ok() && current_pose.y > -1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
// just stop
while(ros::ok()) {
geometry_msgs::Twist move;
move.linear.x = 0;
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
return 0;
}
If you want to reproduce my experiment, you can use the Turtlebot 2 public simulation available in RDS (https://rds.theconstructsim.com/). But you can do it in your own computer too, using this robot.
Basically, you have to consider the current position of the robot to have closed loop. The way it's right now, it relays on time and a very good calibration of the robot, what is not recommended if you want to have a node as generic that could be used in different robots and environments.
Don't forget that you have to add in your CMakeLists.txt and package.xml the dependency nav_msgs before trying to compile.
5 | No.5 Revision |
Hello,
I started from your code and did some changes in order to use an Odometry message to measure how much the robot moved.
I couldn't format the code properly using this editor, so I created a post: http://www.theconstructsim.com/move-certain-distance-turn-move-using-odometry-topic/ But you can see the code below:
#include <ros/ros.h>
<ros/ros.h>
#include <tf/tf.h>
<tf/tf.h>
#include <geometry_msgs/Twist.h>
<geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
<geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
<nav_msgs/Odometry.h>
#include <math.h>
<math.h>
geometry_msgs::Pose2D current_pose;
ros::Publisher pub_pose2d;
void odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
// linear position
current_pose.x = msg->pose.pose.position.x;
current_pose.y = msg->pose.pose.position.y;
// quaternion to RPY conversion
tf::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// angular position
current_pose.theta = yaw;
pub_pose2d.publish(current_pose);
}
int main(int argc, char **argv)
{
const double PI = 3.14159265358979323846;
ROS_INFO("start");
ros::init(argc, argv, "move_pub");
ros::NodeHandle n;
ros::Subscriber sub_odometry = n.subscribe("odom", 1, odomCallback);
ros::Publisher movement_pub = n.advertise("cmd_vel",1); //for sensors the value after , should be higher to get a more accurate result (queued)
pub_pose2d = n.advertise("turtlebot_pose2d", 1);
ros::Rate rate(10); //the larger the value, the "smoother" , try value of 1 to see "jerk" movement
//move forward
ROS_INFO("move forward");
ros::Time start = ros::Time::now();
while(ros::ok() && current_pose.x < 1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//turn right
ROS_INFO("turn right");
ros::Time start_turn = ros::Time::now();
while(ros::ok() && current_pose.theta > -PI/2)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0; //speed value m/s
move.angular.z = -0.3;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
//move forward again
ROS_INFO("move forward");
ros::Time start2 = ros::Time::now();
while(ros::ok() && current_pose.y > -1.5)
{
geometry_msgs::Twist move;
//velocity controls
move.linear.x = 0.2; //speed value m/s
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
// just stop
while(ros::ok()) {
geometry_msgs::Twist move;
move.linear.x = 0;
move.angular.z = 0;
movement_pub.publish(move);
ros::spinOnce();
rate.sleep();
}
return 0;
}
If you want to reproduce my experiment, you can use the Turtlebot 2 public simulation available in RDS (https://rds.theconstructsim.com/). But you can do it in your own computer too, using this robot.
Basically, you have to consider the current position of the robot to have closed loop. The way it's right now, it relays on time and a very good calibration of the robot, what is not recommended if you want to have a node as generic that could be used in different robots and environments.
Don't forget that you have to add in your CMakeLists.txt and package.xml the dependency nav_msgs before trying to compile.