Visualising positions of a detected object in Rviz using find-object-2d [closed]

asked 2017-02-20 07:33:27 -0600

cluelessnigerian gravatar image

I'm using find-object to detect an object together with a Kinect 2 and I'm trying to display the trajectory or points that the object has moved within a room using RVIZ. I just want to draw lines between the last known position of the object and it's current position. I modified tf_example_node.cpp but I'm having a few problems.

My Issues are:

  • I'm using pose.GetRotation() as suggested to find the position of the object in 3D space but the line markers aren't being shown in rviz. A quick rostopic echo shows that visualization_marker isn't publishing anything. Am I doing this right here?

  • Also, I get a warning saying could not find a connection between "map" and "object" as they are not part of the same tree. I'm trying to visualise the movement within a fixed frame /map. How would I achieve this?

This is my code

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <find_object_2d/ObjectsStamped.h>
#include <QtCore/QString>
#include <visualization_msgs/Marker.h>

#include <cmath>

class TfExample
    TfExample() :
        ros::NodeHandle pnh("~");
        pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
        pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);

    ros::NodeHandle nh;
    subs_ = nh.subscribe("objectsStamped", 1, &TfExample::objectsDetectedCallback, this);

    ros::NodeHandle n;
        marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);


// Here I synchronize with the ObjectsStamped topic to
// know when the TF is ready and for which objects
void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr & msg)

            ros::Rate loop_rate(30);

        for(unsigned int i=0; i<msg->; i+=12)
            // get data
            int id = (int)msg->[i];
                 std::string objectFrameId = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString(); // "object_1", "object_2"

                 points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = QString("/object_%1").arg(id).toStdString();
                 points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
                 points.ns = line_strip.ns = line_list.ns = "points_and_lines";
                 points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
                 points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

                 tf::StampedTransform pose;
                 tf::StampedTransform poseCam;

                // Get transformation from "object_#" frame to target frame "map"
                // The timestamp matches the one sent over TF
                tfListener_.lookupTransform(mapFrameId_, objectFrameId, msg->header.stamp, pose);
                tfListener_.lookupTransform(msg->header.frame_id, objectFrameId, msg->header.stamp, poseCam);
            catch(tf::TransformException & ex)

            // Here "pose" is the position of the object "id" in "/map" frame.
            ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
                    id, mapFrameId_.c_str(),
                    pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
                    pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());

            ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
                    id, msg->header.frame_id.c_str(),
                    poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(),
                    poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w());

   = 0;
            line_strip ...
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by cluelessnigerian
close date 2017-02-23 18:13:38.164597


For the second error, what is your TF tree? If you just want the pose according to camera, you may set map_frame_id:=kinect2_base_link.

matlabbe gravatar image matlabbe  ( 2017-02-23 13:37:20 -0600 )edit

@matlabbe, I wasn't publishing tf when running the kinect bridge. Silly error really. I also managed to solve my first question. Thanks for all your help

cluelessnigerian gravatar image cluelessnigerian  ( 2017-02-23 18:15:59 -0600 )edit

Hello @cluelessnigerian....I have similar problem like you at the moment. How did you solve the two questions?`??? Please i need your help...thanks.

Buldoza gravatar image Buldoza  ( 2019-03-12 03:59:48 -0600 )edit