[Hydro] Getting pose of a spawned object in certain frame [closed]

edit

Hi!

I am trying to spawn an object into the scene based on a frame, and afterwards take its position based on that frame. But it is not working when I try to spawn it using a part of the PR2 as frame:

(I put it with rosrun but it is the same thing if I use to code to spawn it)

rosrun gazebo_ros spawn_model -file rospack find pr2_dynamic_motion/worlds/box_big.urdf -urdf -y 1 -model box51 -robot_namespace box.urdf -reference_frame "pr2::base_footprint"

X Y Z should be 2 0 0 but if I take a look to the pose in Gazebo these are quite different, I guess that because the object was spawed using Gazebo's "world" frame.

Same X Y Z using:

rosservice call gazebo/get_model_state '{model_name: box51, relative_entity_name: "pr2::base_footprint"}'

So as this was not working I decided to transform the frame from /world to /base_footprint. But it is not getting the frames, and these exists if I execute "rosrun tf tf_monitor".

It looks like if it is not able to get the available frames. I guess I am initializing well the Transformer. Maybe it is because of the nodeHandler call? I am lost :-(

Frame transformation:

 #include " ros/ros.h "
 #include "gazebo_msgs/GetModelState.h"
 #include < cstdlib >
 #include < string >
 #include < tf/transform_listener.h >
 #include "geometry_msgs/PoseStamped.h"

int main(int argc, char **argv) {
  ros::init(argc, argv, "Get_object_info");
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<gazebo_msgs::GetModelState>("gazebo/get_model_state");
  gazebo_msgs::GetModelState srv;
  srv.request.model_name = "box51";

  srv.request.relative_entity_name = "world";
  if (client.call(srv))
  {
    ROS_INFO_STREAM(srv.request.model_name << " position ");
    ROS_INFO_STREAM(srv.response.pose.position);
  }
  else
  {
    ROS_ERROR("Failed ");
    return 0;
  }

  tf::TransformListener* m_tfListener;

// Version 1 
  m_tfListener = new tf::TransformListener(n, ros::Duration(5), true);

  m_tfListener->waitForTransform("world", "base_footprint",
                                 ros::Time(0), ros::Duration(.50));

  tf::StampedTransform start_transform;
  m_tfListener->lookupTransform("world", "base_footprint",
                                ros::Time(0), start_transform);  

// Version 2
  geometry_msgs::PoseStamped pose_out;
  geometry_msgs::PoseStamped pose_in;

  pose_in.pose = srv.response.pose;
  pose_in.header.frame_id = "/world";
  pose_in.header.stamp = ros::Time::now();  

  std::string frame = "base_footprint";
  m_tfListener = new tf::TransformListener(n, ros::Duration(5), true);

  ROS_INFO_STREAM(m_tfListener->resolve("world")); // This makes nothing!!
  ROS_INFO_STREAM(m_tfListener->resolve("/world")); // This makes nothing!!

  m_tfListener->waitForTransform("/world", frame, ros::Time(0), ros::Duration(3.0));
  m_tfListener->transformPose(frame,
                 pose_in,
                 pose_out); 

  ROS_INFO_STREAM(pose_out.pose.position);

  return 1;                                   

}

Help! :-)