service not moving robot in a non empty world [closed]

asked 2012-07-12 23:12:15 -0500

Gen_Robotino gravatar image

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

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2015-03-26 18:23:31.598396