The C++ equivalent version of the python code.
Launch stage with willow-erratic world and make sure the robot has enough space to move forward and out without hitting any obstacles before turning and getting back.
Run your "out_and_back" node using "rosrun".
/*
* out_and_back.cpp
*
* Created on:
* Author:
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <angles/angles.h>
double x_current = 0;
double y_current = 0;
double theta_current = 0;
tf::TransformListener *odom_listener;
void get_odom()
{
tf::StampedTransform transform;
try
{
odom_listener->lookupTransform("/odom", "/base_footprint", ros::Time(0), transform);
x_current = transform.getOrigin().x();
y_current = transform.getOrigin().y();
theta_current = angles::normalize_angle_positive(tf::getYaw(transform.getRotation()));
ROS_INFO("odom (x, y, theta) : (%f, %f, %f)", x_current, y_current, theta_current);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
}
}
void move(ros::Publisher cmd_VelPublisher, double move_distance, double linearSpeed, int direction)
{
// Current position
double x = x_current;
double y = y_current;
// Initialize the movement command message
geometry_msgs::Twist cmd_velMsg;
// Set the movement command linear speed for forward motion
cmd_velMsg.linear.x = direction * linearSpeed;
// How fast to update the robot's movement?
// Set the equivalent ROS rate variable
ros::Rate rate(10.0);
double d = 0;
// Enter the loop to move the robot
while (d < move_distance && ros::ok())
{
//Publish the Twist message and sleep 1 cycle
cmd_VelPublisher.publish(cmd_velMsg);
ros::spinOnce();
rate.sleep();
// Get the current odometry
get_odom();
// Compute the Euclidean distance from the start position
d = sqrt(pow((x_current - x), 2) + pow((y_current - y), 2));
ROS_INFO("d: %f", d);
}
// Stop the robot
cmd_velMsg.linear.x = 0;
cmd_VelPublisher.publish(cmd_velMsg);
return;
}
void turn(ros::Publisher cmd_VelPublisher, double turn_angle, double angularSpeed, int direction)
{
// Initialize the movement command message
geometry_msgs::Twist cmd_velMsg;
//Set the movement command rotation speed
cmd_velMsg.angular.z = direction * angularSpeed;
// How fast to update the robot's movement?
// Set the equivalent ROS rate variable
ros::Rate rate(10.0);
// Current angle
get_odom();
double last_angle = theta_current;
double angle = 0;
while ((angle < turn_angle) && ros::ok())
{
//Publish the Twist message and sleep 1 cycle
cmd_VelPublisher.publish(cmd_velMsg);
//ROS_INFO("current_theta: %f, angle + turn_angle: %f", angle, (angle + turn_angle));
ros::spinOnce();
rate.sleep();
// Get the current odometry
get_odom();
// Compute the amount of rotation since the last loop
angle += angles::normalize_angle(theta_current - last_angle);
last_angle = theta_current;
ROS_INFO("angle: %f", angle);
}
// Stop turning the robot
cmd_velMsg.angular.z = 0;
cmd_VelPublisher.publish(cmd_velMsg);
}
int main(int argc, char **argv)
{
// Initiate new ROS node named "out_and_back"
ros::init(argc, argv, "out_and_back");
ros::NodeHandle node;
// Publisher to the robot's velocity command topic
ros::Publisher cmd_VelPublisher;
// Advertise a new publisher for the simulated robot's velocity command topic
cmd_VelPublisher = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
odom_listener = new tf::TransformListener();
// Find out if the robot uses /base_link or /base_footprint
try
{
odom_listener->waitForTransform("/odom", "/base_footprint", ros::Time(0), ros::Duration(1.0));
// try
// {
// odom_listener->waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(1.0));
// }
// catch (tf::TransformException ex)
// {
// ROS_ERROR("Cannot find transform between /base_link and /odom :", ex.what());
// return 0;
// }
}
catch (tf::TransformException ex)
{
ROS_ERROR("Cannot find transform between /base_footprint and /odom :", ex.what());
return 0 ...
(more)