Revision history [back]

Hi here I leave a simple example that you can run in any robot with an odom topic that it might clear up some doubts on how this works. Basically that error appears when the result is not correctly set up in the setSucceeded method.

Here you have the GIT

Here you have the ROSJECT

Here you have a Video explaining how to launch it and the basic ideas behind this: VIDEO

And here the main elements of code for those that preffer having it here in the Forum, hope it helps:

MonitorDistance.cpp

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/Empty.h>
#include <nav_msgs/Odometry.h> 
#include <math.h>
#include <action_status_cpp_qanda/ActOdomAction.h>

class ActOdomTurtle {
public:

    int hz;
    float time_to_make_distance; //duration
    bool action_active;
    float x;
    float y;
    bool finish;
    float distance_to_make;
    float hypt;


    ros::NodeHandle nh;
    nav_msgs::Odometry *odom_hold; // I need this shi to hold my nav_msgs/Odom I DONT THINK I DID THIS RIGHT
    ros::Subscriber sub_read;
    ros::Rate *rate_;
    actionlib::SimpleActionServer<action_status_cpp_qanda::ActOdomAction> as_; // i might wanna rename this later
    std::string action_name_; // i need to input this for the action name;
    action_status_cpp_qanda::ActOdomResult result_; //basically in the msg these r the thing im returning

    nav_msgs::Odometry latest_odometry;

    ActOdomTurtle(std::string name):
        as_(nh, name, boost::bind(&ActOdomTurtle::executeCB, this, _1), false), 
        action_name_(name)
    {
        ROS_INFO("Inside ActOdomTurtle Action Init...");

        this->hz = 20;
        this->distance_to_make = 3;
        this->time_to_make_distance = 100;

        this->action_active = false;
        this->finish = false;

        rate_= new ros::Rate(this->hz);

        sub_read = nh.subscribe<nav_msgs::Odometry>("/odom",1,&ActOdomTurtle::giveOdomMod,this);

        as_.start();

        ROS_INFO("Inside ActOdomTurtle Action Init...END");

    }

    void executeCB(const action_status_cpp_qanda::ActOdomGoalConstPtr &gg){

        ROS_INFO("Action has been called...Time Starts Counting to get out of the maze");
        this->action_active = true;

        while (this->time_to_make_distance > 0 && this->finish == false ){ 

            if (as_.isPreemptRequested() || !ros::ok()){         
                ROS_INFO("%s: Preempted", action_name_.c_str());
                as_.setPreempted();
                break;
            } 

            this->finish = this->are_we_out_of_maze();

            time_to_make_distance -= 1/(float)hz; 
            this->rate_->sleep();
            ROS_INFO("the time left is %f", time_to_make_distance);
        }


        // If we finished because we got out of the maze, then its a success.
        if(this->finish) { 
            ROS_INFO("%s: Succeeded", action_name_.c_str());
            as_.setSucceeded(result_);
        }else
        {
            ROS_INFO("%s: Aborted", action_name_.c_str());
            //set the action state to aborted
            as_.setAborted(result_);
        }

        this->action_active = false;
    }

    void giveOdomMod(const nav_msgs::Odometry::ConstPtr &_msg){ 
        /***
         * We Fetch the latest odometry data and save it in a vector 
         * 
         * **/

        this->latest_odometry.header = _msg->header;
        this->latest_odometry.child_frame_id = _msg->child_frame_id;
        this->latest_odometry.pose = _msg->pose;

        if (this->action_active)
        {
            this->result_.result_odom_array.push_back(this->latest_odometry);
        }


    }

    bool are_we_out_of_maze()
    {
        bool result = false;

        // This is a security mesure to guarantee that the saved element in the vector is 
        // what we use for calculating.
        if (!this->result_.result_odom_array.empty())
        {
            nav_msgs::Odometry last_odom = this->result_.result_odom_array.back();
            float last_x = last_odom.pose.pose.position.x;
            float last_y = last_odom.pose.pose.position.y;

            float hypt = sqrt(pow(last_x,2)+pow(last_y,2));
            // Because the robot starts at the origin we dont have to save the init position
            // To then compare with the latest.
            if (hypt >= this->distance_to_make ){
                ROS_WARN("SUCCESS Distance from origin %f >= %f", hypt, this->distance_to_make);
                result = true;
            }else
            {
                ROS_ERROR("NOTYET Distance from origin %f >= %f", hypt, this->distance_to_make);
            }
        }else
        {
            ROS_ERROR("Not data yet inside this->result_.result_odom_array");
        }


        return result;
    }

};

int main(int argc, char** argv){

  ros::init(argc, argv, "MonitorDistance_Node");

  ROS_INFO("Starting Action...");
  ActOdomTurtle waft("monitor_distance");
  ROS_INFO("Starting Action...END, now spining");
  ros::spin();

  return 0;
}