include "ros/ros.h"

include "gazebo_msgs/SetModelState.h"

include "gazebo_msgs/GetModelState.h"

include "gazebo_msgs/GetPhysicsProperties.h"

include

include

include

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/get_model_state");

ros::ServiceClient gphspro=n.serviceClient("/gazebo/get_physics_properties");

ros::ServiceClient client = n.serviceClient("/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(a*pi/180); sna = sin(a*pi/180);

e = sqrt(x*x + y*y);

//Υπολογισμός γραμμικής ταχύτητας - u u = (e*g*csa);

//Υπολογισμός γωνιακής ταχύτητας - ω w = (k*a + ((g*csa*sna*(a + h*th))/a));

twist.angular.z = w; twist.linear.x = sqrt(u*u - twist.linear.y*twist.linear.y); twist.linear.y = sqrt(u*u - twist.linear.x*twist.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/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;

}

}

Comments

Please update your question with new information instead of posting it as an answer. ROS Answers is not a forum or mailing list, so answers may be reordered by date or votes and won't make sense to the next person with this problem. Thanks for helping to keep ROS Answers organized and easy to use!

Asked by Eric Perko on 2012-04-26 13:39:13 UTC