ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
![]() | 1 | initial version |
int main(int argc, char** argv)
{
ros::init(argc, argv, "kouna_to_asistola2");
ros::NodeHandle n;
double f, t, th, a, x, y, e, p, csa, sna, u, w, Xfinal[3], Xinit[3];
double pi=3.141592654;
int g = 3; int h = 1; int k = 6;
int i;
geometry_msgs::Pose pose;
geometry_msgs::Twist twist;
twist.linear.x = 0.0; twist.linear.y = 0.0;
Xinit[0]=0; Xinit[1]=0; Xinit[2]=0;
Xfinal[0]=6.0; Xfinal[1]=7.0; Xfinal[2]=86.0;
for(i=1; i<100000; i+=1) {
ros::ServiceClient gmscl=n.serviceClient<gazebo_msgs::getmodelstate>("/gazebo/get_model_state");
ros::ServiceClient gphspro=n.serviceClient<gazebo_msgs::getphysicsproperties>("/gazebo/get_physics_properties");
ros::ServiceClient client = n.serviceClient<gazebo_msgs::setmodelstate>("/gazebo/set_model_state");
gmscl.waitForExistence(); gphspro.waitForExistence(); client.waitForExistence();
gazebo_msgs::GetModelState getmodelstate; getmodelstate.request.model_name ="turtlebot"; gmscl.call(getmodelstate);
gazebo_msgs::GetPhysicsProperties getphysicsproperties; gphspro.call(getphysicsproperties);
pose.position.z = 0.0;
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
t=0.0001;
ROS_INFO("position.x = %f",pose.position.x); ROS_INFO("position.y = %f",pose.position.y); ROS_INFO("orientation = %f",pose.orientation.z);
x = Xfinal[0] - Xinit[0];
y = Xfinal[1] - Xinit[1];
f = Xfinal[2] - Xinit[2];
p = atan2(y,x); p = p*180/pi;
th = Xfinal[2] - p;
a = th - f;
if(a<0) a=-a;
csa = cos(api/180); sna = sin(api/180);
e = sqrt(xx + yy);
//Υπολογισμός γραμμικής ταχύτητας - u u = (egcsa);
//Υπολογισμός γωνιακής ταχύτητας - ω w = (ka + ((gcsasna(a + h*th))/a));
twist.angular.z = w; twist.linear.x = sqrt(uu - twist.linear.ytwist.linear.y); twist.linear.y = sqrt(uu - twist.linear.xtwist.linear.x);
if(Xinit[0] <= Xfinal[0]) pose.position.x = getmodelstate.response.pose.position.x+twist.linear.x*t)); else pose.position.x = getmodelstate.response.pose.position.x+0.0;
if(Xinit[1] <= Xfinal[1]) pose.position.y = getmodelstate.response.pose.position.y+twist.linear.y*t)); else pose.position.y = getmodelstate.response.pose.position.y+0.0;
if(Xinit[2] <= Xfinal[2]) pose.orientation.z = getmodelstate.response.pose.orientation.z+(twist.angular.z*t); else pose.orientation.z = getmodelstate.response.pose.orientation.z+0.0;
Xinit[0]=pose.position.x; Xinit[1]=pose.position.y; Xinit[2]=pose.orientation.z;
// ros::ServiceClient client = n.serviceClient<gazebo_msgs::setmodelstate>("/gazebo/set_model_state");
gazebo_msgs::SetModelState setmodelstate;
gazebo_msgs::ModelState modelstate;
modelstate.model_name ="turtlebot";
modelstate.pose = pose;
modelstate.twist = twist;
setmodelstate.request.model_state=modelstate;
if (client.call(setmodelstate))
{
ROS_INFO("BRILLIANT AGAIN!!!");
}
else
{
ROS_ERROR("Failed to call service for setmodelstate ");
return 1;
}
u=0.0; twist.angular.z=0.0;
}
}