Ask Your Question
0

Problem with my launch file

asked 2012-04-05 02:12:45 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

I have created a client (named kouna_to_asistola_client.cpp located into the turtlebot2 pkg), that moves turtlebot in gazebo. it works with the rosrun command. i want to make it work with the roslaunch command. but when i try to include the node in the launch file i get the following error:

[ERROR] [1333630193.884536832]: Failed to call service for setmodelstate

[kouna_to_asistola-5] process has died [pid 26078, exit code 1].
log files: /home/megalicious/.ros/log/d6c8e168-7f1d-11e1-bb54-14dae9034270/kouna_to_asistola-5*.log

this is my client:

#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_asistola");

    ros::NodeHandle n;

    for (double t=0.1; t<=20000; t+=0.001)
    {


    ros::ServiceClient gmscl=n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    gazebo_msgs::GetModelState getmodelstate;
    getmodelstate.request.model_name ="turtlebot";
    gmscl.call(getmodelstate);

    ros::ServiceClient gphspro=n.serviceClient<gazebo_msgs::GetPhysicsProperties>("/gazebo/get_physics_properties");
    gazebo_msgs::GetPhysicsProperties getphysicsproperties;
    gphspro.call(getphysicsproperties);

    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

    twist.linear.x = 0.020;
    twist.linear.y = 0.020;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;

    pose.position.x = getmodelstate.response.pose.position.x + 0.05;
    pose.position.y = getmodelstate.response.pose.position.y + 0.05;
    pose.position.z = 0.0;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;
    pose.orientation.z = 0.0;

    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;
      }

  }


}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
4

answered 2012-04-07 09:00:32 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

one potential problem I can see is that you are not waiting for the service servers to come up first. secondly, you might not want to use service calls for fast pose updates, rather, I recommend using latched ros topics or writing a gazebo plugin to accomplish tthis. Here is an example plugin template. Nevertheless, below is your code with some minor fixes.

#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_asistola");

  ros::NodeHandle n;

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

  for (double t=0.1; t<=20000; t+=0.001)
  {

    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;

    twist.linear.x = 0.020;
    twist.linear.y = 0.020;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;

    pose.position.x = getmodelstate.response.pose.position.x + 0.05;
    pose.position.y = getmodelstate.response.pose.position.y + 0.05;
    pose.position.z = 0.0;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;
    pose.orientation.z = 0.0;

    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;
    }
  }
}
edit flag offensive delete link more
0

answered 2012-04-26 04:13:08 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

thank u for your answer. i made some changes:

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

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.001;

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

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(api/180); sna = sin(api/180);

e = sqrt(xx + yy);

u = (egcsa);

twist.angular.z = (ka + ((gcsasna(a + h*th))/a));

if(pose.position.x <= Xfinal[0]) pose.position.x = getmodelstate.response.pose.position.x+cos(pose.orientation.zpi/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.zpi/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::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;
  }

}

}

btw, i have the launch file:

<launch>

<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>

<node name="spawn_turtlebot_model" pkg="gazebo" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model turtlebot" respawn="false" output="screen"/>

<node name="Kuna_to_asistola2" pkg="turtlebot2" type="kouna_to_asistola2" args="-u $(find turtlebot2)/src" respawn="false" output="screen"/>

</launch>

but, i think it is ok..

anw, now i get the error:

GetModelState: model [turtlebot] does not exist Failed to call service for setmodelstate

any ideas??

thank u in advance!

edit flag offensive delete link more

Comments

@grge Please ask a new question for a new problem.

tfoote gravatar imagetfoote ( 2012-05-11 15:24:32 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-04-05 02:12:45 -0500

Seen: 772 times

Last updated: Apr 26 '12