class escapeTurtle {
    protected:
    int rate_hz;
    float time_limit; //duration
    bool success;
    float x;
    float y;
    bool finish;
    float constant;
    int cycles;
    // i could also input the exact points and do the subtraction
    int final_el;
    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<my_turtlebot_actions::EscapeAction> as_; // i might wanna rename this later
    std::string action_name_; // i need to input this for the action name;
    my_turtlebot_actions::EscapeResult result_; //basically in the msg these r the thing im returning

    public:
    escapeTurtle(std::string name):
        as_(nh, name, boost::bind(&escapeTurtle::executeCB, this, _1), false), 
        action_name_(name) //this means that action_name_ = string name; 
    {
       //############################################################################
        time_limit = 200; //this will be seconds of how long I will allow the robot to escape the maze
        rate_hz = 20;
        constant = 8; //this is magnitude I will use
        success = true;
        finish = true; //this is true because once it is false we will break the executeCB loop// THIS IS ME FINISHING THE MAZE OKAY W/E

        sub_read = nh.subscribe<nav_msgs::Odometry>("odom",100,&escapeTurtle::giveOdom,this);
        rate_= new ros::Rate(rate_hz);
        cycles = (int)time_limit * rate_hz; //this rn is = 60*20; // this will be my limit kind of for a for loop
        odom_hold = new nav_msgs::Odometry[cycles]; //dynamically allocated array of nav_msgs size cycles
        as_.start();

    }

    ~escapeTurtle(){
      delete [] odom_hold;
    }

    void waste_time(){
        rate_->sleep();
    }

    void executeCB(const my_turtlebot_actions::EscapeGoalConstPtr &gg){ //I DONT HAVE A GOAL SO I WONT PUT IT HERE //MY OUTPUT IS THIS ARRAY WE ARE CREATING FROM SUBSCRIBING TO nav_msgs/Odom
    ROS_INFO("WTF IS GOING ON 1");
    //we could do a thing where we put everything in 
    while (time_limit > 0 && finish == true ){

    if (as_.isPreemptRequested() || !ros::ok()){         // set the action state to preempted
        ROS_INFO("%s: Preempted", action_name_.c_str());
        as_.setPreempted();
        success = false;
        break;
      } //if the action is called to a stop early

      time_limit -= 1/(float)rate_hz; //this is changing time_limit to go to 0
      this->waste_time();
      //ROS_INFO("the time left is %f" ,time_limit);
    }

      if(success) { //i think the for loop would go besst in here // we only have one result to give though
      //************** I DELETED THE RESULT PART 
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      as_.setSucceeded(result_); //IF WHAT I DID IN RESULT WORKED THEN IM GUUCHI
      //this is how the client knows were done with this shi... by sending the result 
     }
  }

  void giveOdom(const nav_msgs::Odometry::ConstPtr& msg){ //msg is a pointer 
     if (!as_.isActive()){ // I WANT IT ALL TO START AT THE SAME TIME
         return;
      }
        for (int i = 0; i < cycles; i++ ){
        odom_hold[i] = *msg; //XDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD
        //result_.result_odom_array[i] = *msg; //oh what LOLS this worked //THIS DID NOT WORK CAUSE IT CAUSED THE FUCKING ERROR 
        //ROS_INFO("This is the i %d", i);
        x = odom_hold[i].pose.pose.position.x;
        //ROS_INFO("This is the x pos %f", x);
        y = msg->pose.pose.position.y;
        hypt = sqrt(pow(x,2)+pow(y,2)); //vector // MY MAG IS HARDSTUCK SINCE I RAN CODE...
        //ROS_INFO(" Mag distance %f",hypt ); //I COULD JUST USE THE ACTUAL DISTANCE FOR MY WHILE LOOP
        final_el = i;
        this->waste_time();
        if (hypt >= constant ){
        finish = false;
        break;
        }
      }
     }

};

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

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

  escapeTurtle waft("turtleAction"); //this is the action naming
  //waft.givemeX();
  ros::spin();

  return 0;
}

I DONT KNOW WHY I AM READING THE SAME ODOM POINT WITH THIS CODE.

asked 09 Oct, 02:14

josephleung78%40gmail.com's gravatar image

josephleung7...
01
accept rate: 0%

edited 12 Nov, 21:12

marcoarruda's gravatar image

marcoarruda ♦♦
1764

Be the first one to answer this question!
toggle preview

Follow this question

By Email:

Once you sign in you will be able to subscribe for any updates here

By RSS:

Answers

Answers and Comments

Markdown Basics

  • *italic* or _italic_
  • **bold** or __bold__
  • link:[text](http://url.com/ "title")
  • image?![alt text](/path/img.jpg "title")
  • numbered list: 1. Foo 2. Bar
  • to add a line break simply add two spaces to where you would like the new line to be.
  • basic HTML tags are also supported

Question tags:

×12
×11
×3

question asked: 09 Oct, 02:14

question was seen: 73 times

last updated: 12 Nov, 21:12

powered by OSQA