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%

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:

×11
×11
×3

question asked: 09 Oct, 02:14

question was seen: 10 times

last updated: 09 Oct, 02:14

powered by OSQA