ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

problem with client node [closed]

asked 2012-04-26 09:27:53 -0500

grge gravatar image

hey. i have created a client node in order to control turtlebot in gazebo. something is not ok though. the roswtf command gives these warnings:

WARNING The following node subscriptions are unconnected: * /gazebo: * /gazebo/set_model_state * /set_update_rate * /gazebo/set_link_state * /set_hfov * /cmd_vel

WARNING The following nodes are unexpectedly connected: * /gazebo->/rosout (/rosout) * /kouna_to_asistola2->/rosout (/rosout)

WARNING These nodes have died: * spawn_turtlebot_model-3

WARNING No tf messages

any ideas??

thanks

edit retag flag offensive reopen merge delete

Closed for the following reason duplicate question by tfoote
close date 2012-04-26 14:02:09

Comments

What is your node supposed to do, and in what way does it not work? Just the output from roswtf is not sufficient to debug most problems.

Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2012-04-26 11:00:42 -0500 )edit

my node takes a final pose of the robot (x,y,theta), and produces the linear and angular velocities until the robot reaches the goal. so i have to get the robot pose in each time step in order to implement the control.!

grge gravatar image grge  ( 2012-04-26 11:53:19 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2012-04-26 11:55:58 -0500

grge gravatar image

include "ros/ros.h"

include "gazebo_msgs/SetModelState.h"

include "gazebo_msgs/GetModelState.h"

include "gazebo_msgs/GetPhysicsProperties.h"

include <stdio.h>

include <math.h>

include <stdlib.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, 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;

}

}

edit flag offensive delete link more

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!

Eric Perko gravatar image Eric Perko  ( 2012-04-26 13:39:13 -0500 )edit

Question Tools

Stats

Asked: 2012-04-26 09:27:53 -0500

Seen: 845 times

Last updated: Apr 26 '12