void Pose::updateOdometry()
{
gettimeofday(&endtime,NULL);//get a timestamp that corresponds to the end of the previous action.
//calculate distance travelled based on linear velocity and the duration of the previous movement
distance=velocity*(((endtime.tv_sec * 1000000) + (endtime.tv_usec)) - ((starttime.tv_sec * 1000000) + (starttime.tv_usec)))/1000000.0;
//depending on previous movement update odometry.
if (MOVING_FORWARD==g_prev_movement){
robotPose.x+=distance*cos(robotPose.theta);
robotPose.y+=distance*sin(robotPose.theta);
}
else if(MOVING_BACKWARD==g_prev_movement){
robotPose.x+=distance*cos(robotPose.theta+M_PI);
robotPose.y+=distance*sin(robotPose.theta+M_PI);
}
else if (TURNING_LEFT==g_prev_movement){
robotPose.theta+=ang_velocity*(((endtime.tv_sec * 1000000) + (endtime.tv_usec)) - ((starttime.tv_sec * 1000000) + (starttime.tv_usec)))/1000000.0;
robotPose.theta=normRad(robotPose.theta);
}
else if(TURNING_RIGHT==g_prev_movement){
robotPose.theta-=ang_velocity*(((endtime.tv_sec * 1000000) + (endtime.tv_usec)) - ((starttime.tv_sec * 1000000) + (starttime.tv_usec)))/1000000.0;
robotPose.theta=normRad(robotPose.theta);
}
else if(STOPPED==g_prev_movement){
}
/*set the start time of the new movement and set the previous movement to current movement for the next call of the function*/
g_prev_movement=g_movement;
starttime.tv_sec=endtime.tv_sec;
starttime.tv_usec=endtime.tv_usec;
cout<<"New Position: (x,y,angle): "<<"("<<robotPose.x<<", "<<robotPose.y<<", "<<robotPose.theta<<")"<<endl;
}
DirTime Pose::shiftToGoal(SITE id_site)
{
//get coordinates of the next goal
goal_y=0;
goal_x =0;
goal_y=sites[id_site].readyPoint.y;
goal_x=sites[id_site].readyPoint.x;
//find the angle of the line that connects the estimated robot position and the target point of next site.
angle=atan2(goal_y-robotPose.y,goal_x-robotPose.x);
/*
cout<<"goal x: "<<goal_x<<endl;
cout<<"goal y: "<<goal_y<<endl;
cout<<"Pose x: "<<robotPose.x<<endl;
cout<<"Pose y: "<<robotPose.y<<endl;
*/
//cout<<"After arctan"<<angle<<endl;
angle-=robotPose.theta;//subtract the current bearing of the robot from the angle to find how much to turn
//cout<<"Angle: "<<angle<<endl;
angle=normRad(angle);//normalize so that the angle we find is between minus pi and pi
//cout<<"NormRad: "<<angle<<endl;
ret_val.time=abs((angle/ang_velocity)*1000);//calculate time for turn based on turning angle
//decide on direction of turn depending on the turning angle sign
if (angle<0){
ret_val.direction=TURNING_RIGHT;
}
else{
ret_val.direction=TURNING_LEFT;
}
return ret_val;//return the direction and time of the turn
}
geometry_msgs::Twist
int main(int argc, char **argv){
ros::init(argc, argv, "subP3AT");
ros::NodeHandle n;
//função para ouvir os dados da odometria chamando a função Callback acima
ros::Subscriber sub = n.subscribe("/RosAria/pose",1000, chatterCallback);
geometry_msgs::Twist vel_msg;
ros::Publisher velocity_publisher = n.advertise<geometry_msgs::Twist>("RosAria/cmd_vel",100);
double Ex=0.0,Ey=0.0;
double u=0.0,w=0.0, alpha = 0.0;
double a=0.0, fb1, fb2;
double Xp,Yp,thetaP;
double MPontos [3][3] = {{1,0,0},{1,1,pi/2},{2,2,0}}; //given points to follow
for(int i=0; i<3;i++){
Xp = MPontos[i][0];
yP = Mpontos[i][1];
thetaP = Mpontos[i][2];
while(ros::ok()){ //loop ate pressionar ctrl+C
//ROS_INFO("xpositionDentroDoWhil: [%f]",posx);
//ROS_INFO("passouasdeeeWhile");
Ex = posx - Xp;
Ey = posy - Yp;
thetaM = thetaO - thetaP;
xM = (cos(thetaP)*Ex + sin(thetaP)*Ey);
yM = (-sin(thetaP)*Ex + cos(thetaP)*Ey);
a = coordCircular(xM,yM);
alpha = normRad(thetaM-thetaD(xM,yM));
fb1 = funcB1(xM,yM,thetaM, alpha);
u = -gama2(gama1,a)*fb1*a;
fb2 = funcB2(xM,yM,thetaM);
w = (-fb2*u) - (k*alpha);
/*
ROS_INFO("xM, yM, thetaM: [%f, %f, %f]",xM,yM, thetaM);
ROS_INFO("thetaO, thetaP, alpha, : [%f, %f, %f]",thetaO, thetaP, alpha);
ROS_INFO("fb1, fb2: [%f, %f]",fb1, fb2);
ROS_INFO("a, alpha, u, w: [%f, %f,%f, %f]",a, alpha,u,w);
*/
//velocidades lineares
//seta a velocidade liner no eixo x para a frente do robo
vel_msg.linear.x = u;
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
//velocidades angulares
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = w; //seta o valor de rotação do robo p3at
//if(chatterCallback->posx>0.7){break;}
velocity_publisher.publish(vel_msg);
if(Ex<0.2 or Ey<0.2) break;
ros::spinOnce();
}
//fica fazendo loop ate apertar ctrl+c
}
ros::spin();
return 0;
}
