I have special image array data and I want convert it to PointCloud2.
But I can't understand meaning of PointCloud2's parameter, so I can't do converting.
Every pixel of Image array data have straight distance in the floor and have the angle from the center of bottom. ( angle and distance in real world)
This is my questions.
1. What is point_step, row_step, height, width in PointCloud2,msg?
2. What is offset and count in PointField.msg?
3. How cant I convert image array data to PointCloud2?
↧
How can I convert image array data to PointCloud2?
↧
laserscan to pointcloud timestamp issue
Dear all,
I want to convert laserscans (sick lms511) to pointcloud2. I have found a topic on how to perform this conversion here :
https://answers.ros.org/question/11232/how-to-turn-laser-scan-to-point-cloud-map/?comment=283322#post-id-283322
My problem is that my laserscans are saved in a rosbag file that was generated from original data (csv) using a third party software. As a consequence, when I use the solution above (transformLaserScanToPointCloud) I get an error due to the difference in timestamps (the laserscans are older than bagtime which is also older than current time).
I tried to fix this issue with the creation of a new rosbag file and setting sim time to false in order to ignore sim time. This did not fix the issue. Then I've repeated this operation with sim time = true, unfortunately I still have the same error.
Please, can you help me with this issue ?
Thanks.
↧
↧
Providing PointCloud2 data from ZED camera
I am trying to run a program that requires PointCloud2 data using only the ZED camera, no LASER. Depthcloud to Laserscan is said to work well with ZED, so could I combine this with the Laserscan to Pointcloud routine in one node, would that work? It appears that the latter code isn't really a ROS node but a routine to be included in
another program, correct? The ZED can make Pointcloud output itself but it is too slow, like 2 Hz.
↧
Accessing values while iterating points in sensor_msgs::PointCloud2 from sensor_msgs::PointCloud2ConstIterator
I am writing a node that subscribes to a topic of type `sensor_msgs::PointCloud2`.
In the callback I want to iterate the points in the point cloud using `sensor_msgs::PointCloud2ConstIterator`.
This is how I think it should be implemented (which seems to work, no serious testing so far, though):
void callback(sensor_msgs::PointCloud2ConstPtr const& msg) {
sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"),
iter_y(*msg, "y"),
iter_z(*msg, "z");
while (iter_x != iter_x.end()) {
// TODO: do something with the values of x, y, z
std::cout << *iter_x << ", " << *iter_y << ", " << *iter_z << "\n";
++iter_x; ++iter_y; ++iter_z;
}
}
Is this about the recommended way of iterating the points in a point cloud?
I've seen there was also an overload of the [element access `operator[](...)`](http://docs.ros.org/kinetic/api/sensor_msgs/html/classsensor__msgs_1_1impl_1_1PointCloud2IteratorBase.html#a9e878e61602940a92de2b908f505c69f). While I was trying to figure out how element access exactly works, I wrote this little callback to get an impression:
void callback(sensor_msgs::PointCloud2ConstPtr const& msg) {
sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x");
std::cout << *iter_x << ", "
<< iter_x[0] << ", "
<< iter_x[1] << ", "
<< *(iter_x+1) << "\n";
}
I got an output like this:
1.04852, 1.04852, -1.78216, 1.54717
I wondered, shouldn't the last two values be identical?
↧
Cutting off a pointcloud2 at a certain range
I am working on a mapping problem with a laser scanner. I want to cut off the 20% of points that are furthest away from the robot. So this distance should be variable for each time steps. Depending on how close we are to walls in certain areas.
I came up with the following approach:
void PcFilter::pcCallback(const sensor_msgs::PointCloud2ConstPtr &pointcloud_in)
{
unsigned int num_points = pointcloud_in->width;
std::vector ranges(num_points);
sensor_msgs::PointCloud2 pointcloud_out;
int cut_off_idx = int(num_points*0.8);
pointcloud_out.header = pointcloud_in->header;
pointcloud_out.fields = pointcloud_in->fields;
pointcloud_out.height = pointcloud_in->height;
pointcloud_out.width = cut_off_idx;//+1
pointcloud_out.point_step = pointcloud_in->point_step;
pointcloud_out.row_step = pointcloud_out.width*pointcloud_out.point_step;
pointcloud_out.is_bigendian = pointcloud_in->is_bigendian;
pointcloud_out.is_dense = pointcloud_in->is_dense;
sensor_msgs::PointCloud2ConstIterator iter_x(*pointcloud_in, "x");
sensor_msgs::PointCloud2ConstIterator iter_y(*pointcloud_in, "y");
int count = 0;
//iterate over pointcloud an save the distance from the points in the xy plane
for(;iter_x != iter_x.end();
++iter_x, ++iter_y)
{
ranges[count] = hypot(*iter_x, *iter_y);
count++;
}
std::sort(std::begin(ranges), std::end(ranges));
float cut_off_dist = ranges[cut_off_idx];
float point_dist;
sensor_msgs::PointCloud2ConstIterator iter_xx(*pointcloud_in, "x");
sensor_msgs::PointCloud2ConstIterator iter_yy(*pointcloud_in, "y");
//iterate over pointcloud again and fill data in new pointcloud
for(;iter_xx != iter_xx.end(); ++iter_xx, ++iter_yy)
{
point_dist = hypot(*iter_x, *iter_y);
if (point_dist <= cut_off_dist)
{
//todo fill in data
//pointcloud_out.data =0;
}
}
pub_pc2_.publish(pointcloud_out);
}
What I cant seem to accomplish is actually filling the data field with the corresponding points. I saw sth about memcpy, but I could find an extensive tutorial. Appreciate your help!
↧
↧
PointCloud2 read_points unpack_from problem
Hey, I'm working on a object recognition framework that uses deep learning methods.
Given the locations of the objects in the image (bounding box), I use the PointCloud publised by a Kinect 2, to locate those.
Currently, I'm working with ROS Kinetic and I'm developing in python.
Sometimes, trying to obtain the xyz points of the bounding box, using the read_points method I receive the following error:
> File "/home/some1/catkin_ws/src/sem_mapper_isr/sem_mapper_markers/nodes/object_mapper.py", line 205, in get_object_3D_points
points = np.array(list(pc2.read_points(self.curr_pc,field_names = ("x", "y", "z"), skip_nans=True, uvs=bb_pts)))
File "/opt/ros/kinetic/lib/python2.7/dist-packages/sensor_msgs/point_cloud2.py", line 83, in read_points
p = unpack_from(data, (row_step * v) + (point_step * u))
error: unpack_from requires a buffer of at least 12 bytes
This error doesn't happen everytime I try to obtain the xyz points. Sometimes, there is no problem.
The code is extensive, I will try to show their important parts for this case.
**Imports:**
import rospy
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2
self.pc_xyz_sub = rospy.Subscriber(sub_pointcloud_topic, PointCloud2, self.pointcloud_callback, queue_size=2)
def pointcloud_callback(self, pointcloud_msg):
self.pc_message_locker.acquire()
self.curr_pc = pointcloud_msg
self.pc_message_locker.release()
def get_object_3D_points(self,width_min,width_max,height_min,height_max,view_path):
bb_pts=((i,j) for i in xrange(width_min,width_max+1) for j in xrange(height_min, height_max+1))#pixels of the image to verify in the pointcloud
self.pc_message_locker.acquire() #lock the pc message -> dont change/access current pc message at same time
points = np.array(list(pc2.read_points(self.curr_pc,field_names = ("x", "y", "z"), skip_nans=True, uvs=bb_pts)))
self.pc_message_locker.release()
↧
kinect2 dont take all point clouds
Hi everyone
my kinect 2 does not save the entire point cloud that is displayed in the rviz or in the viewer,
ignores surfaces and some elements(walls, objects, ground, etc) the strange thing is that in the viewer the points if there are.
for example
I use this code to capture the XYZ RGB data.
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import struct
import ctypes
import time, os, sys
path_data = os.getcwd() + "/data/raw"
flag=0
def saveCloud_txt(fileName):
fileName= fileName + "_" + time.strftime("%d-%m-%y")+'-'+time.strftime("%I-%M-%S")
completeName = os.path.join(path_data, fileName + '.txt')
f = open(completeName,"a") #opens file with name of "[fileName].txt"
dataFile= "%" + "X"+"\t"+ "Y"+"\t"+ "Z" +"\t"+ "R" +"\t" + "G"+"\t"+ "B" + "\n"
f.write(dataFile)
return f
def callback(data):
global flag
rospy.loginfo('Data acquiring')
pc = pc2.read_points(data,skip_nans=True)
int_data=list(pc)
for P in int_data:
x=P[0]
z=-P[1]
y=P[2]
s=struct.pack('>f', P[3])
i=struct.unpack('>l',s)[0]
pack = ctypes.c_uint32(i).value
R=(pack & 0x00FF0000)>> 16
G=(pack & 0x0000FF00)>> 8
B=(pack & 0x000000FF)
dataFile = str(x)+"\t"+ str(y)+"\t"+ str(z) +"\t"+ str(R)+"\t" +str(G) +"\t"+ str(B)+ "\n"
f.write(dataFile)
print "X: "+str(x)+ "; Y: "+str(y)+"; Z: "+str(z) +"; R: "+str(R)+" ; G:"+str(G)+"; B:"+str(B)
del x,y,z,R,G,B
print "--------------------------------------------------------------------------------------------"
flag=1
def ros_listener():
global flag
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('Kinect_Capture', anonymous=True)
rospy.Subscriber('/kinect2/sd/points', PointCloud2, callback)
while flag==0:
nop_op = 0
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
if __name__ == '__main__':
fileName = raw_input('File name: ')
print "5"
time.sleep(1)
print "4"
time.sleep(1)
print "3"
time.sleep(1)
print "2"
time.sleep(1)
print "1"
time.sleep(1)
f = saveCloud_txt(fileName)
ros_listener()
↧
pointcloud2 and path planning
currently running ubuntu 16.04, ros kinetic, using gazebo 7 simulation with a sawyer (intera 5.2), running kinect camera.
I want to take a point from a point cloud (type `PointCloud2`) taken from the kinect frame. point would be of type `PointXYZRGBNormal`.
I want to take the point, use a transformation to get the point in the world frame, then input that into a script i wrote that performs inverse kinematics to determine a pose for sawyer.
My script takes a point input in the form of a position (xyz) and a quaternion (xyzw). I also want to be able to subscribe to the pointcloud rostopic and index through the points to select a single point and its relevant data.
can anyone suggest how to take the `PointXYZRGBNormal` and convert the information to a position and quaternion in the world frame?
Please let me know if more information is required.
Any help/suggestions would be much appreciated.
~ Luke
↧
pointcloud2 transform c++/python
running kinetic, ubuntu 16.04, gazebo 7, sawyer simulator with intera 5.2, kinect/openni camera
warning: i am very new to ROS, C++ and Python.
trying to transform a point cloud (type `PointCloud2`, PointXYZRGBNormal) from the kinect camera frame to the base frame of sawyer and then publish the transformed pointcloud. I'm doing this so that i can select a point in the world frame for end-effector of sawyer to move to.
I have a script (in `C++`) that's publishing the point cloud mentioned above to a rostopic. I'll just briefly recount what i've tried to do.
1. The scripts for moving sawyer are writting in `Python`. So i tried to write a script that would subscribe to the pointcloud rostopic. In my python script, i was able to write a listener to the transformation tree and i was also able to write a lookupTransform in the frames that i'm dealing with. However, when i tried to perform the actual transformation, the transformPointCloud function i was using was not compatible with PointCloud2. I wasn't sure where to go from there and i wasn't sure which packages i should be using.
2. I tried to perform the transformation inside the `C++` script that is publishing the pointcloud i'm working with. the publishing function actually takes the raw kinect point cloud (`PointXYZRGB`) and calculates normals for each point then re-pubslishes the pointcloud with normals (`PointXYZRGBNormal`). both the kinect pointcloud and the pointcloud with normals are in the form of `PointCloud2` as stated above. the trouble i'm having with writing in C++ is that i'm not sure if i should subscribe to the publisher within the same script to do the transform, or just pass the point cloud messages between functions, or just to transform the point cloud messages inside the publishing function.
can anyone provide a good, and sufficiently detailed, example of `pointcloud2` transformation in Python and/or C++?
Preferably, an example that makes use of a transform listener would be most useful since the transformation tree is already set up inside the sawyer simulation.
I'm also getting confused with which tf packages i should be using since all the examples i've been looking at dont explicitly say which packages the functions come from. Adding to that, i'm also not sure of which packages i have nor do i know how to update them. So help regarding packages, package update and installation would be very helpful.
I know that was a lot of info and not much code to work off of. I wouldn't know which bits of code to share. If anything needs clarification, please leave a comment and i'll clarify as much as i can. i'll also share snippets of code in comments as necessary.
thanks in advance!
~ Luke
↧
↧
"right_arm_base_link" passed to lookupTransform argument target_frame does not exist.
ros kinetic, ubuntu 16.04, gazebo 7, intera 5.2 (for sawyer robot), kinect camera in simulation.
trying to transform pointcloud from kinect frame to base from of a sawyer robot. i've coded for this inside another void function that estimates the normals for each point in a point cloud.
trying to use `transformPointCloud` from `/tf`. code compiles properly, but i get the error in the title of the question.
This is the code i'm using.
tf::TransformListener listener;
tf::StampedTransform transform;
// Perform point cloud transformation
listener.lookupTransform("/right_arm_base_link", "/depth_link", ros::Time(0), transform); // based on line 129 /tf/tf.h
pcl::PointCloud::Ptr trans_cloud (new pcl::PointCloud);
pcl_ros::transformPointCloud("/right_arm_base_link", *cloud_with_normals, *trans_cloud, listener); // based on line 131 /pcl_ros/transforms.h
// Publish transformed point cloud
sensor_msgs::PointCloud2 ros_trans_out;
pcl::toROSMsg( *trans_cloud, ros_trans_out);
tf_pub.publish (ros_trans_out);
`*cloud_with_normals` is of type `PointCloud2` and is my point cloud in. `*trans_cloud` is my point cloud out, also `PointCloud2`. the goal is to be able to publish the transformed point cloud to a rostopic (publisher has been omitted from the question). my concern is that `right_arm_base_link` is not being recognised, but it appears in the transformation tree when i used `$ rostopic echo /tf`.
I can also use the following to determine the actual transformation between `right_arm_base_link` and `depth_link`:
$ rosrun tf tf_echo right_arm_base_link depth_link
and it will yield:
At time 2358.846
- Translation: [0.547, 0.183, 0.017]
- Rotation: in Quaternion [-0.081, 0.701, 0.080, 0.704]
in RPY (radian) [-0.336, 1.567, -0.108]
in RPY (degree) [-19.257, 89.763, -6.202]
At time 2359.846
- Translation: [0.547, 0.183, 0.017]
- Rotation: in Quaternion [-0.081, 0.701, 0.080, 0.704]
in RPY (radian) [-0.335, 1.567, -0.107]
in RPY (degree) [-19.206, 89.763, -6.146]
can anyone suggest what i'm doing wrong or how to fix this issue? i will provide more detail if and when needed.
Thanks in advance!
~ Luke
↧
Publish pointCloud2 from csv files for LOAM
Hi everyone,
I have laser scans in the form of .csv files. I want to read these .csv files and publish the scans as Pointcloud2 messages, so that I can feed them to LOAM for registration.
Currently, I am trying to do it with Matlab, but there doesn't seem to be a way to publish pointcloud2 message from Matlab?
So, could you please recommend me how to do that either in Matlab or by any other way?
I understand that somehow I need to set the fields of the PointCloud2 object using the scan data from .csv files, but I am not sure how to do that. So, your help is greatly appreciated because I could not find the solution to this problem online anywhere.
~Shantnu
↧
Can't display occupied voxels octomap using pointcloud2
Fairly new to this so let me know if there's a better way of doing things!
I'm attempting dynamic collision avoidance with a Kinect V1 and the ur5 moveit package on ROS kinetic. The PC is running Ubuntu 16.04 and i have the following packages installed:
- camera_pose
- octomap_mapping
- octomap_ros
- OpenNI
- openni_camera
- perception_pcl
- robotiq
- trac_ik
- universal_robot
- ur_modern_driver
- ur_scripts
I've been following the ROS tutorial on perception/configuration (link below) in an effort to have moveit process the point cloud into an octomap and display the occupied voxels: http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/perception_configuration/perception_configuration_tutorial.html
And am also aware of this other thread which is extremely similar, however i wasn't able to achieve a working result using just the information here: https://answers.ros.org/question/195770/moveit-collision-avoidance-with-point-cloud-2/
I have Openni working so can visualise the pointcloud2 okay in moveit and have completed all the steps in the above, including altering the sensor_manager.launch.yaml files. However when i echo the planning scene topic, `/move_group/monitored_planning_scene`, i am getting nothing. This is despite trying a number of static transform publishers to arrange everything with respect to `/world` (I would post images of my tf trees but i dont have enough karma).
Can anyone shed any light on this?
↧
Order of point clouds to use while merging (ICP)
My team and I have built the following setup (view from above). It's a 3x3x3m aluminum grid with 4 Kinects placed at height 2.1~2.2m and are facing downwards (at 20~30 degrees). The angle between the neighbors is 90 degrees. At this point I need to merge point clouds from all these cameras (camera calibration, manual transformations have been applied). The question is: do I merge them to one single camera one by one, or follow clockwise/counterclockwise direction, or there is a better way to merge them?
↧
↧
How to save OctoMap data everytime?
Hi,
I'm getting a 3D map using the OctoMap server package. But As the vehicle moves in this mapping, the map updates itself to the exact location where the vehicle is moving. What I want to do is not delete the previous data of the map until the vehicle is gone from the starting position like 2d mapping. How can ı do this? Could you please help me.
 This is the 3D map I get using PointCloud2.
↧
Moveit Octomap Not Allowing Command Planning or Execution
Hi all,
I've been trying to hook up Moveit so that my robot can avoid obstacles using openni_tracker and openni_launch. I've added octomap capabilities to my demo.launch file as:
↧
Coloring of pcd-pointcloud in Rviz
Hi!
I am very new to ROS, so please take that into account when answering :)
(Ubuntu 14.04 64bit in a VirtualBox with Windows Host, ROS Indigo)
**What I did:**
I used
rosrun pcl_ros pcd_to_pointcloud myFilename.pcd 0.1
to publish a topic containing my pcl-pointcloud as a PointCloud2 message. The output of this call is:
> [ INFO] [1449049889.334492450]:> Publishing data on topic /cloud_pcd> with frame_id /base_link. [ INFO]> [1449049889.438294770]: Loaded a point> cloud with 307200 points (total size> is 4915200) and the following> channels: x y z rgba.
As one can correctly see, the pointcloud also contains color information. I now tried to visualize the point cloud using Rviz. For this purpose I set the Fixed Frame to "/base_link" and added a "PointCloud2"-Display whose topic I set to "/cloud_pcd".
**Problem description:**
I was only able to see the point cloud when setting the color to something else than RGB8. Why? There should be color information and I'd like my point cloud nicely colored :) How can I fix this?
↧
Rtabmap PointCloud2 frame oscillates when viewed in RVIZ
The PointCloud2 topic's frame keeps switching back and forth wrt to the robot's frame, as seen in
https://imgur.com/a/NosBsOx. What could be the cause?
↧
↧
Why cannot launch node of type [laser_geometry/laser_geometry]
I need to transfer from `sensor_msgs::LaserScan` to `sensor_msgs::PointCloud2`. I found laser_geometry package, (http://wiki.ros.org/laser_geometry). Because there is no ROS API, I found a link which helps. The link is (https://answers.ros.org/question/11232/how-to-turn-laser-scan-to-point-cloud-map/). I compiled the .cpp file same to the link:
#include "laser_geometry/laser_geometry.h"
#include
#include
#include
#include
#include "ros/ros.h"
class My_Filter{
public:
My_Filter();
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
private:
ros::NodeHandle node_;
laser_geometry::LaserProjection projector_;
tf::TransformListener tfListener_;
ros::Publisher point_cloud_publisher_;
ros::Subscriber scan_sub_;
};
My_Filter::My_Filter(){
scan_sub_ = node_.subscribe ("/scan", 100, &My_Filter::scanCallback, this);
point_cloud_publisher_ = node_.advertise ("filtered_cloud", 100, false);
tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}
void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 cloud;
projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tfListener_);
point_cloud_publisher_.publish(cloud);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_filter");
My_Filter filter;
ros::spin();
return 0;
}
from my cmakelist.txt and package.xml, I wrote a launch file
`target_link_libraries(laser_geometry ${Boost_LIBRARIES} ${tf_LIBRARIES})`, `laser_geometry `
↧
Understand PointCloud2 msg (data array)
Hi everyone,
I used rostopic echo and received a bunch of PointCloud2 Msg, but I have a hard time understand it especially for the "data" array.
Here is one example:
header:
seq: 0
stamp:
secs: 1527193450
nsecs: 13024000
frame_id: "velodyne"
height: 1
width: 1
fields:
-
name: "x"
offset: 0
datatype: 7
count: 1
-
name: "y"
offset: 4
datatype: 7
count: 1
-
name: "z"
offset: 8
datatype: 7
count: 1
-
name: "intensity"
offset: 16
datatype: 7
count: 1
is_bigendian: False
point_step: 32
row_step: 32
data: [218, 49, 151, 64, 105, 199, 245, 64, 50, 184, 192, 63, 0, 0, 128, 63, 1, 0, 64, 64, 252, 127, 0, 0, 182, 0, 0, 0, 0, 0, 0, 0]
is_dense: True
I have read through the documents for PointCloud2 but did not find helpful information for understand the "data". I want to extract point coordinate as (x,y,z) from it. Looking forward to some help!
↧
ROS2: topic subscribed but not received
I have a device driver that publishes a Pointcloud2 message at 30 Hz.
I can subscribe the message in Rviz and I can see the correct pointcloud.
When I move the camera in 3D view the pointcloud disappears and I see `Showing [0] points from [0] messages` in `Points` under the Pointcloud Status.
If I open a console and I try to subscribe to the topic by command line using
`$ ros2 topic echo /zed_node/point_cloud/cloud_registered`
it seems that the message is not published indeed.
I'm pretty sure that my node is publishing it because if I subscribe to it and remove the subscription I can see the total number of subscribers changing.
For further information I'm using Bouncy compiled from source under Ubuntu 16.04 and the following QOS settings:
rmw_qos_profile_t depth_qos_profile = rmw_qos_profile_default; // Default QOS profile
depth_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
depth_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
depth_qos_profile.depth = 1;
depth_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
Thank you for help
Walter
↧