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 '18, 02:14
josephleung7...
0●1
accept rate:
0%