ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

marker position and orientation

asked 2018-12-16 05:27:55 -0600

dinesh gravatar image

updated 2019-01-05 08:31:34 -0600

right now i am detecting a moving object in 3d point cloud and than sending the voxels of moved object in rviz. but when i view it in rviz, its color and oreantation is not coming correctly, what should i do to remove this problem? Here is the screen shot, where i've put a bag infront of kinect sensor, but it is not displayed correctly in rviiz: image description

im using ubuntu 18.04, ros melodic. Here is the full code:

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <visualization_msgs/Marker.h>
#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <fstream>

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <thread>
ros::Publisher marker_publisher;
int frame_index = 0;
using namespace std;
int x[200000];
void thread_function(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloudB,vector<int> v,int p0) {
    for(size_t p1=0;p1<v.size() && ros::ok();++p1) {
        int p0p1 = sqrt( pow(cloudB->points[v[p1]].x-cloudB->points[v[p0]].x,2)                
        +pow(cloudB->points[v[p1]].y-cloudB->points[v[p0]].y,2)
        +pow(cloudB->points[v[p1]].z-cloudB->points[v[p0]].z,2) ) * 1000;
        if(p0p1>10) {
            for(size_t p2=0;p2<v.size() && ros::ok();++p2) {
                int p0p2 = sqrt( pow(cloudB->points[v[p2]].x-cloudB->points[v[p0]].x,2)                
                +pow(cloudB->points[v[p2]].y-cloudB->points[v[p0]].y,2)
                +pow(cloudB->points[v[p2]].z-cloudB->points[v[p0]].z,2) ) * 1000;
                int p1p2 = sqrt( pow(cloudB->points[v[p2]].x-cloudB->points[v[p1]].x,2)                
                +pow(cloudB->points[v[p2]].y-cloudB->points[v[p1]].y,2)
                +pow(cloudB->points[v[p2]].z-cloudB->points[v[p1]].z,2) ) * 1000;
                if(p0p2>10 && p1p2>10) {
                }    
            }    
        }       
    }    
    x[p0] = 3;
    cout<<"ended thread="<<p0<<endl;
}

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
    frame_index++;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZRGB> );
    pcl::fromROSMsg(*input,*cloudB);

    // Initializing Marker parameters which will be used in rviz
    vector<visualization_msgs::Marker> line_list, marker, text_view_facing;
    line_list.resize(4); marker.resize(4); text_view_facing.resize(4);
    for(int i=0;i<line_list.size();i++) {
        marker[i].header.frame_id = line_list[i].header.frame_id = text_view_facing[i].header.frame_id = "/X3/base_link";
        marker[i].header.stamp = line_list[i].header.stamp = text_view_facing[i].header.stamp =ros::Time();
        marker[i].ns = line_list[i].ns = text_view_facing[i].ns ="lines";
        marker[i].action = line_list[i].action = text_view_facing[i].action = visualization_msgs::Marker::ADD;
        marker[i].pose.orientation.w = line_list[i].pose.orientation.w = text_view_facing[i].pose.orientation.w = 1;
        marker[i].id = i+4;
        line_list[i].id = i;
        marker[i].type = visualization_msgs::Marker::POINTS;
        line_list[i].type = visualization_msgs::Marker::LINE_LIST;
        line_list[i].color.r = 1; line_list[i].color.g = 1; line_list[i].color. b = 1; line_list[i].color.a = 1;
        marker[i].scale.x = 0.003;
        marker[i].scale.y = 0.003;
        marker[i].scale.z ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2018-12-16 10:53:50 -0600

There are a few things to fix here.

The points of the object are in the wrong place because you're copying the x, y and z in the wrong order. So your switching the x and z values. That's an easy one to fix.

The color values are also switched but there is another thing to fix here too. The color problem is caused because the point cloud and RVIZ marker describe color values in different ranges. The point cloud uses 0-255 while the RVIZ marker uses 0.0 - 1.0 for this reason you'll need to divide the color values by 255.0 . The '.0' at the end is very important it forces c++ to perform a floating point divide as opposed to an integer divide.

Hope this gets this working for you.

edit flag offensive delete link more

Comments

ok the color is coming correctly after i divide them with 255.0 but the position is still not coming correctly .

dinesh gravatar image dinesh  ( 2019-01-04 21:45:30 -0600 )edit

Can you show us your updated code so we can see what you've changed.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-05 04:55:36 -0600 )edit

ok, i've updated the code.

dinesh gravatar image dinesh  ( 2019-01-05 08:31:43 -0600 )edit

The frame Id of the marker you're creating is hard coded to "/X3/base_link", if the point cloud has a different frame id, then this would explain the problem. Can you look at the original point cloud in RVIZ and find out what it's frame_id is?

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-05 10:10:15 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2018-12-16 05:27:55 -0600

Seen: 1,510 times

Last updated: Jan 05 '19