I am currently working on the finishing touches of the rosject associated with the ROS Basics in 5 Days (C++). However, my action server client callbacks are not executing as intended. I believe it is a threading issue, but my research and debugging attempts have not yielded results.
I have 3 nodes with a launch file that launches all 3.
- find_wall.cpp (service server)
- follow_wall.cpp (service client, action client, basic subscriber/publisher)
- record_odom.cpp (action server)
follow_wall should wait for the find_wall service to finish, then call the record_odom server, then perform a subscriber/publisher callback that navigates the robot around the room. As the robot is traveling around the room (laser_Callback), feedbackCb() should also be printing the total distance traveled and doneCb() should execute at the end of the lap when a result is returned.
I have tried creating asynchronous spinners and multithreaded spinners to no avail.
Please let me know how to coordinate callbacks between follow_wall.cpp and record_odom.cpp. Thank you!
find_wall.cpp:
#include "ros/duration.h"
#include "ros/init.h"
#include "ros/rate.h"
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "rosject/FindWall.h"
#include <bits/stdc++.h>
#include <sensor_msgs/LaserScan.h>
class controller {
public:
controller(){
pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
sub = nh.subscribe("/scan", 10, &controller::laserCallback, this);
my_service = nh.advertiseService("/find_wall", &controller::find_Callback, this);
}
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
// Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall.
laser_readings.clear();
// creating vector of laser distances
for (int i = 0; i < msg -> ranges.size(); i++) {
laser_readings.push_back(msg -> ranges[i]);
}
}
bool find_Callback(rosject::FindWall::Request &req,
rosject::FindWall::Response &res)
{
ROS_INFO("find_Callback has been called"); // We print an string whenever the Service gets called
// Rotate the robot until the front of it is facing the wall. This can be done by publishing an angular velocity until front ray is the smallest one.
status_update = "Rotating to face wall.";
ROS_INFO("%s\n", status_update.c_str());
while((laser_min < 359) || (laser_min > 361)){
// finding index of min value in vector
laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
if (laser_min > 360){
// if wall on the left, turn left
vel.angular.z = 0.1;
} else if (laser_min < 360){
// if wall on the right, turn right
vel.angular.z = -0.1;
} else {
// if facing wall, stop
vel.angular.z = 0;
}
pub.publish(vel);
ros::spinOnce();
}
// Move the robot forward until the front ray is smaller than 30cm.
vel.angular.z = 0;
vel.linear.x = 0.1;
pub.publish(vel);
ROS_INFO("\n Min Element = %d", laser_min);
status_update = "Moving toward wall.";
ROS_INFO("%s\n", status_update.c_str());
while(laser_readings[360] > 0.3){
ros::spinOnce();
}
// Now rotate the robot again until ray number 270 of the laser ranges is pointing to the wall.
vel.angular.z = 0.1;
vel.linear.x = 0;
pub.publish(vel);
ROS_INFO("\n Min Element = %d", laser_min);
status_update = "Turning parallel with wall.";
ROS_INFO("%s\n", status_update.c_str());
while((laser_min < 269) || (laser_min > 271)){
laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
ros::spinOnce();
}
// stop robot, ready for follow_wall
vel.angular.z = 0;
vel.linear.x = 0;
pub.publish(vel);
ROS_INFO("\n Min Element = %d", laser_min);
status_update = "In position for follow_wall.";
ROS_INFO("%s\n", status_update.c_str());
// At this point, consider that the robot is ready to start following the wall.
// Return the service message with True.
ROS_INFO("find_Callback completed");
res.wallfound = true;
return true;
}
private:
ros::Subscriber sub;
ros::Publisher pub;
ros::NodeHandle nh;
ros::ServiceServer my_service;
std::string status_update;
geometry_msgs::Twist vel;
std::vector<float> laser_readings;
int laser_min;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "find_wall");
controller ctrl;
ros::spin();
return 0;
}
follow_wall.cpp
#include "ros/publisher.h"
#include <ros/ros.h>
#include <iostream>
#include <string>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>
#include "rosject/FindWall.h"
#include <rosject/OdomRecordAction.h>
#include <actionlib/client/simple_action_client.h>
class controller {
public:
controller(): nh(), spinner(0){
spinner.start();
ros::service::waitForService("/find_wall"); // wait for service to be running
ros::ServiceClient find_wall_service = nh.serviceClient<rosject::FindWall>("/find_wall");
rosject::FindWall srv; // Create an object of type srv
while(find_wall_service.call(srv) == false){
ROS_INFO("Failure calling service /find_wall");
}
ROS_INFO("Successfully called service /find_wall");
// call action server to start recording odom values
actionlib::SimpleActionClient<rosject::OdomRecordAction> client("record_odom", true);
client.waitForServer(); // Waits until the action server is up and running
rosject::OdomRecordGoal goal; // Creates a goal to send to the action server
goal.success = false; // Fills the goal. Indicates, take pictures along 10 seconds
client.sendGoal(goal, // sends the goal to the action server
boost::bind(&controller::doneCb, this, _1, _2),
boost::bind(&controller::activeCb, this),
boost::bind(&controller::feedbackCb, this, _1));
sub = nh.subscribe("/scan", 1000, &controller::laserCallback, this);
pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
vel.angular.z = 0;
}
void doneCb(const actionlib::SimpleClientGoalState& state,
const rosject::OdomRecordResultConstPtr& result)
{
ROS_INFO("The robot has completed a lap! Great job, robot.");
ROS_INFO("The robot started at x = %f, y = %f, and theta = %f", result->list_of_odoms[0].x, result->list_of_odoms[0].y, result->list_of_odoms[0].z);
int final = result -> list_of_odoms.size();
//using final as index, so need to reduce by 1 since c++ indexing starts at 0.
final -= 1;
ROS_INFO("The robot started at x = %f, y = %f, and theta = %f", result->list_of_odoms[final].x, result->list_of_odoms[final].y, result->list_of_odoms[final].z);
ros::shutdown();
}
void activeCb()
{
ROS_INFO("Odom values are now being recorded.");
}
void feedbackCb(const rosject::OdomRecordFeedbackConstPtr& feedback)
{
ROS_INFO("[Feedback] The robot has traveled %f meters.", feedback->current_total);
}
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
mylocation = msg -> ranges[180];
vel.linear.x = 0.1;
//continue straight
if (mylocation > 0.2 and mylocation < 0.3) {
vel.angular.z = 0;
status_update = "straight";
}
//turn left to move away from the wall
if (mylocation < 0.2 ) {
vel.angular.z = 0.14;
status_update = "away_from";
}
//turn right to move closer to the wall
if (mylocation > 0.3 ) {
vel.angular.z = -0.1;
status_update = "to";
}
// turn left if wall ahead
if (msg -> ranges[360] < 0.7 ) {
vel.angular.z = 0.4;
status_update = "corner";
}
ROS_INFO("%s\n", status_update.c_str());
//ROS_INFO("%f\n", mylocation);
pub.publish(vel);
//ros::spinOnce();
}
private:
ros::Subscriber sub;
ros::Publisher pub;
ros::NodeHandle nh;
geometry_msgs::Twist vel;
std::string status_update;
double mylocation;
ros::AsyncSpinner spinner;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "follow_wall");
controller ctrl;
ros::waitForShutdown();
//spinner.spin();
//ros::spin();
return 0;
}
record_odom.cpp
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/Empty.h>
#include <rosject/OdomRecordAction.h>
#include <string>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point32.h>
class recordOdom
{
protected:
ros::NodeHandle nh_;
// NodeHandle instance must be created before this line. Otherwise strange error occurs.
actionlib::SimpleActionServer<rosject::OdomRecordAction> as_;
std::string action_name_;
// create messages that are used to publish feedback and result
rosject::OdomRecordFeedback feedback_;
rosject::OdomRecordResult result_;
ros::Subscriber odom;
float calculatedDistance;
float totalDistance = 0;
float startDistance;
int i = 0;
geometry_msgs::Point32 coords;
bool exit;
bool enter;
ros::AsyncSpinner spinner;
public:
recordOdom(std::string name):
nh_(),
spinner(0),
as_(nh_, name, boost::bind(&recordOdom::executeCB,this,_1), false),
action_name_(name)
{
spinner.start();
odom = nh_.subscribe("/odom", 10, &recordOdom::odomCallback, this);
as_.start();
}
~recordOdom(void)
{
}
float calculateDistance(float x1, float y1, float x2, float y2){
return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
coords.x = msg -> pose.pose.position.x;
coords.y = msg -> pose.pose.position.y;
coords.z = msg -> pose.pose.orientation.w;
}
void executeCB(const rosject::OdomRecordGoalConstPtr &goal)
{
// helper variables
ros::Rate r(1);
bool success_;
exit = false;
enter = false;
while(!enter){
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success_ = false;
break;
}
result_.list_of_odoms.push_back(coords);
// new point - previous point = distance traveled since last sub call
// increment onto totalDistance
if(i > 0){
totalDistance += calculateDistance(result_.list_of_odoms[i-1].x,result_.list_of_odoms[i-1].y,result_.list_of_odoms[i].x, result_.list_of_odoms[i].y);
}
// distance from the starting point
startDistance = calculateDistance(result_.list_of_odoms[0].x,result_.list_of_odoms[0].y,result_.list_of_odoms[i].x, result_.list_of_odoms[i].y);
// has the robot left the starting zone?
if(startDistance > 0.5){
// leaves the starting zone
exit = true;
}
// has the robot left the starting zone before and is now back within the starting zone?
if((exit == true) && (startDistance < 0.5)){
// reenters the starting zone - lap complete!
enter = true;
}
feedback_.current_total = totalDistance;
as_.publishFeedback(feedback_);
i += 1;
ros::spinOnce();
r.sleep();
}
if(success_)
{
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "record_odom");
recordOdom rOdom("record_odom");
ros::waitForShutdown();
return 0;
}
