Robotics StackExchange | Archived questions

service not moving robot in a non empty world

I am using a service client to move a robot around a world, and it successfully works for an empty world. The problem is when I launch a non empty world, the robot does not register any of the movements. Does anyone have anyone idea as to why the service client cannot communicate with the robot when not an empty world?

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "gazebo_msgs/SetModelState.h"
#include "gazebo_msgs/ModelState.h"

#include 

int main(int argc, char **argv)
{

  ros::init(argc, argv, "pose_publisher");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise("gazebo/set_model_state", 1000);
  ros::Rate loop_rate(10);
  int count = 0;
  bool alternate = true;
  while (ros::ok())
  {

    geometry_msgs::Pose pose;

    if(alternate){
        pose.position.x = 2.8;
        pose.position.y = 5.3;
        pose.position.z = 0.0;
        pose.orientation.x = 0.0;
        pose.orientation.y = 0.0;
        pose.orientation.z = 0.0;
        alternate = false;
    }
    else{
        pose.position.x = 1.68;
        pose.position.y = 1.68;
        pose.position.z = 0.0;
        pose.orientation.x = 0.0;
        pose.orientation.y = 0.0;
        pose.orientation.z = 0.0;
        alternate = true;
    }

    gazebo_msgs::ModelState modelstate_msg;
    modelstate_msg.model_name = "robo";
    modelstate_msg.pose = pose;

    gazebo_msgs::SetModelState setmodelstate_msg;

    // Publish robot pose
    ROS_INFO("New Pose:" );
    ROS_INFO("Pose x =%f", modelstate_msg.pose.position.x );
    ROS_INFO("Pose x =%f", modelstate_msg.pose.position.y );
    ROS_INFO("Pose x =%f", modelstate_msg.pose.position.z );

    chatter_pub.publish(modelstate_msg);
    //setmodelstate_msg.request.model_state = modelstate_msg;

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

Asked by Gen_Robotino on 2012-07-12 23:12:15 UTC

Comments

Answers