durovsky / marker_pose_detection

Marker pose detection in ROS using Aruco library and pal_robotics Aruco ROS wrapper. Chessboard pose detection function is available as well
https://www.youtube.com/watch?v=RcMGsNf_CoM
BSD 2-Clause "Simplified" License
23 stars 17 forks source link

subscribe to /Estimated_marker ros topic ? #4

Closed Abduoit closed 5 years ago

Abduoit commented 6 years ago

Hi

I can see the ros topic /Estimated_marker which gives me the pose of the marker. I want to subscribe to this pose and publish it in rviz as TF. I wrote the following .cpp script but there's error with it, any help ll be appreciated ?

#include <ros/ros.h>

#include <tf/tf.h>  
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <viewpoint_estimation_lib.h>

void printPose(const visualization_msgs::Marker::ConstPtr& msg)
{   
    tf::Pose marker_pose_in_camera_;    

    marker_pose_in_camera_.setOrigin(tf::Vector3(msg.pose.pose.position.x,
                             msg.pose.pose.position.y,
                             msg.pose.pose.position.z));

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "viewpoint_estimation");

    ros::NodeHandle nh;

    ros::Subscriber pose_sub = nh.subscribe("Estimated_marker", 1000, printPose);

    ros::spin();

    return 0;

}