Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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 <sstream>

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

  ros::init(argc, argv, "pose_publisher");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<gazebo_msgs::modelstate>("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;
}