Mahe's profile - activity

I am trying to get pose of the camera and set it with respect to the world frame. I want to get the frame_id from msg, so that I can set multiple cameras with respect to world frame dynamically.

I am using Asus Xtion pro live, so I launch ar_track_alvar with pr2_indiv_no_kinect.launch.

This is what I have done,

Launch file,

<launch>
    <arg name="marker_size" default="4.4" />
    <arg name="max_new_marker_error" default="0.08" />
    <arg name="max_track_error" default="0.2" />
    <arg name="cam_image_topic" default="/camera1/rgb/image_rect_color" />
    <arg name="cam_info_topic" default="/camera1/rgb/camera_info" />    
    <arg name="output_frame" default="/camera1_link" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
</launch>

My ros Node,

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include<tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include<iostream>
#include <string>

void cb(ar_track_alvar_msgs::AlvarMarkers req) {

  tf::TransformBroadcaster tf_br;
  tf::TransformListener listener;
  static  tf::Transform transform;

  if (!req.markers.empty()) {
    tf::Quaternion q(req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w);
    transform.setOrigin( tf::Vector3(ceil(req.markers[0].pose.pose.position.x), ceil(req.markers[0].pose.pose.position.y), ceil(req.markers[0].pose.pose.position.z)) );
    transform.setOrigin( tf::Vector3(req.markers[0].pose.pose.position.x, req.markers[0].pose.pose.position.y, req.markers[0].pose.pose.position.z) );
    transform.setRotation(tf::Quaternion( req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w));

    try{

        // this doesn't prints the frame id.
        ROS_INFO("req.header.frame_id . . . . . .. .  .. ", req.header.frame_id.c_str());

      if(req.header.frame_id.compare("/camera1_link"))
      {
          ROS_INFO("this gets printed . . ");
        // this works . . I mean string comparision returns true.
        // I want to set frame_id to the tf tree.

       //   listener.waitForTransform(req.header.frame_id, "world", ros::Time::now(), ros::Duration(1.0));
      //    tf_br.sendTransform(tf::StampedTransform(transform.inverse(), ros::Time::now(), "world", req.header.frame_id));

      }

    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
  }
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "camera_tf_pose");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("ar_pose_marker", 1, &cb);

  ros::spin();
  return 0;

}

Output of rosrun camera_tf_pose camera_tf_pose

[ INFO] [1475225608.355125575]: req.header.frame_id . . . . . .. .  .. 
[ INFO] [1475225608.355185064]: this gets printed . . 
[ INFO] [1475225608.454772325]: req.header.frame_id . . . . . .. .  .. 
[ INFO] [1475225608.454802236]: this gets printed . . 
[ INFO] [1475225608.555007653]: req.header.frame_id . . . . . .. .  .. 
[ INFO] [1475225608.555137160]: this gets printed . .

Output of rostopic echo /ar_pose_marker

   markers: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1475225585
        nsecs: 290621273
      frame_id: /camera1_link
    id: 14
    confidence: 0
    pose: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: ''
      pose: 
        position: 
          x: 0.310138838061
          y: -0.0777276835864
          z: -0.00489581265903
        orientation: 
          x: 0.158053463521
          y: -0.431284842866
          z: 0.021097283333
          w: 0.888013170859

when I uncomment the following lines,

 //   listener.waitForTransform(req.header.frame_id ...