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

edit

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
{
public:
    TfExample() :
        mapFrameId_("/map"),
        objFramePrefix_("object")
    {
        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);

    if(msg->objects.data.size())
    {
        for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
        {
            // get data
            int id = (int)msg->objects.data[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;

            try
            {
                // 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)
            {
                ROS_WARN("%s",ex.what());
                continue;
            }

            // 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());

            points.id = 0;
            line_strip ...