turtlebot simulation: controller via client-node [closed]

asked 2012-04-26 07:19:23 -0500

grge gravatar image

updated 2012-04-26 13:59:58 -0500

tfoote gravatar image

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason Question does not follow our guidelines for questions. Please see: http://wiki.ros.org/Support for more details. by tfoote
close date 2012-04-26 14:06:07

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.

tfoote gravatar image tfoote  ( 2012-04-26 14:05:58 -0500 )edit

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

squigglylines gravatar image squigglylines  ( 2015-10-20 10:27:10 -0500 )edit

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

squigglylines gravatar image squigglylines  ( 2015-10-20 10:27:30 -0500 )edit