Robotics StackExchange | Archived questions

turtlebot simulation: controller via client-node

hello guys. i am trying to simulate a control problem for turtlebot in gazebo. the controller takes the pose of the robot in each timestep and produces the linear and angular velocities until the robot reachew it's goal. so i made a client node, in order to rosrun it after i roslaunch the turtlebot in an empty world. unfortunatelly, when i rosrun my node i get no response from the robot, and the following warning:

Gazebo ROS Create plugin. NaN in d2. Step time: 0.03. WD: 0.07. Velocity: -nan.

i think the problem is that my client program doesn't get the robot pose properly. here is my client code. please help me. thank u in advance. cheers!

#include "ros/ros.h"
#include "gazebo_msgs/SetModelState.h"
#include "gazebo_msgs/GetModelState.h"
#include "gazebo_msgs/GetPhysicsProperties.h"

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, Xfinal[3];

double pi=3.141592654;

int g = 3;
int h = 1;
int k = 6;

int i;

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();

    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

pose.position.x=0.0;
pose.position.y=0.0;
pose.orientation.z=0.0;

for(i=1; i<100000; i++)
{

gazebo_msgs::GetModelState getmodelstate;
    getmodelstate.request.model_name ="turtlebot";
    gmscl.call(getmodelstate);

gazebo_msgs::GetPhysicsProperties getphysicsproperties;
    gphspro.call(getphysicsproperties);


    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

    t=0.1;

Xfinal[0]=6.0;
Xfinal[1]=7.0;
Xfinal[2]=86.0;

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] - pose.position.x;  

y = Xfinal[1] - pose.position.y;  

f = Xfinal[2] - pose.orientation.z;  

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 = (e*g*csa);

twist.angular.z = (k*a + ((g*csa*sna*(a + h*th))/a));

ROS_INFO("x = %f",x);
ROS_INFO("y = %f",y);
ROS_INFO("a = %f",a);
ROS_INFO("csa = %f",csa);
ROS_INFO("sna = %f",sna);
ROS_INFO("e = %f",e);
ROS_INFO("u = %f",u);



  if(pose.position.x <= Xfinal[0])
    pose.position.x = getmodelstate.response.pose.position.x+(cos(pose.orientation.z*pi/180)*(u*t));
  else
    pose.position.x = pose.position.x;

  if(pose.position.y <= Xfinal[1])
    pose.position.y = getmodelstate.response.pose.position.y+(sin(pose.orientation.z*pi/180)*(u*t));
  else
    pose.position.y = pose.position.y;

    pose.position.z = 0.0;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;

 if(pose.orientation.z <= Xfinal[2])
    pose.orientation.z = getmodelstate.response.pose.position.z+(twist.angular.z*t);
 else
    pose.orientation.z = pose.orientation.z;

ROS_INFO("=========================================");  
ROS_INFO("position.x(2) = %f",pose.position.x);
ROS_INFO("position.y(2) = %f",pose.position.y);
ROS_INFO("orientation(2) = %f",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!!!");
       ROS_INFO("%f",getphysicsproperties.response.time_step);
       ROS_INFO("%f",modelstate.pose.position.x);
       ROS_INFO("%f",modelstate.pose.position.y);
       ROS_INFO("%f",modelstate.twist.linear.x);
       ROS_INFO("%f",getmodelstate.response.pose.position.y);
      }
     else
      {
       ROS_ERROR("Failed to call service for setmodelstate ");
       return 1;
      }

  }

}    

Asked by grge on 2012-04-26 07:19:23 UTC

Comments

This is not a good question. See our guidelines at: http://www.ros.org/wiki/Support Please debug it yourself to isolate where you are having the problem and then make a minimal test case which reproduces your problem to try to get help.

Asked by tfoote on 2012-04-26 14:05:58 UTC

if you to subscribe to '/gazebo/link_states' there will be a link 'mobile_base::base_footprint'. This gives the position of the turtlebot. If the linkState==data and 'mobile_base::base_footprint'==robotid, then data.pose[robotid].position gives the x,y,z position

Asked by squigglylines on 2015-10-20 10:27:10 UTC

data.pose[robotid].orientation gives the orientation as a quaternion.

Asked by squigglylines on 2015-10-20 10:27:30 UTC

Answers