pointcloud_to_laserscan rotation of scan line

Question

I am running pointcloud_to_laserscan . When I display the pointcloud2 in rviz after rotation the camera frame is displays correctly along the rviz ground plane. However the laser from pointcloud_to_laserscan produces a scan line that does not register to the cloud2 in rviz .

from rostopic list

/camera_info
/clicked_point
/clock
/cmd_vel
/depth/image_raw
/depth/points
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gmapping_node/entropy
/image_raw
/image_raw/compressed
/image_raw/compressed/parameter_descriptions
/image_raw/compressed/parameter_updates
/image_raw/compressedDepth
/image_raw/compressedDepth/parameter_descriptions
/image_raw/compressedDepth/parameter_updates
/image_raw/theora
/image_raw/theora/parameter_descriptions
/image_raw/theora/parameter_updates


 self.msgAry  = [self.createStaticTransform("base_footprint",  0,  0,  0.1,  "base_link", 0, 0, 0),   \
                 self.createStaticTransform("base_link",  -0.13,  -0.13,  0.1,  "left_wheel", 0, 0, 0),   \
                 self.createStaticTransform("base_link",  -0.13,  0.13,  0.1,  "right_wheel", 0, 0, 0),   \
                 self.createStaticTransform("base_link",  -0.1,  0,  0.1,  "tower_link", 0, 0, 0),   \
                 self.createStaticTransform("tower_link", 0.0,  0,  0.2,  "camera_link", 0, 0, 0),  \
                 self.createStaticTransform("camera_link", 0,  0,  0,  "camera_frame_optical", 0, 0, 0),  \
                 self.createStaticTransform("camera_link", 0, 0,  0,  "rrbot/camera_frame",  -1.57079633,  0,  -1.57079633)]  
 
 self.pub_tf.publish(self.msgAry)




  <!-- ******************************************************************************************** -->

  <node name="pointcloud_to_laserscan_node" pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node"  output="screen" respawn="true">
      <remap from="cloud_in" to="/depth/points"/>
      <rosparam>
            target_frame: "camera_frame_optical"
            tolerance: 0.01
            min_height: 1.0
            max_height: 10.0

            angle_min: -1.5708 # -M_PI/2
            angle_max: 1.5708 # M_PI/2
            angle_increment: 0.00436717644334
            scan_time: 0.3333
            range_min: 0.1
            range_max: 30.0
            use_inf: false

            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
            # 0 : Detect number of cores
            # 1 : Single threaded
            # 2->inf : Parallelism level
            concurrency_level: 0
       </rosparam>
  </node>  
  

eg: of scan output

header: 
  seq: 530
  stamp: 
    secs: 435
    nsecs: 155000000
  frame_id: rrbot/camera_frame
angle_min: -1.57079994678
angle_max: 1.57079994678
angle_increment: 0.00436717644334
time_increment: 0.0
scan_time: 0.333299994469
range_min: 0.10000000149
range_max: 30.0
ranges: [31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 31.0, 0.8586809635162354, 31.0, 0.8496611714363098, 0.8406707644462585, 0.831710159778595, 0.8041678667068481, 0.7952516674995422, 0.7863677144050598, 0.7961777448654175, 0.7775167226791382, 0.7686992287635803, 0.7599161863327026, 0.7511681914329529, 0.7323523759841919, 0.7236281037330627, 0.7149416208267212, 0.7443765997886658, 0.7062936425209045, 0.6976853609085083, 0.7275895476341248, 0.6701399087905884, 0.6994651556015015, 0.6616077423095703, 0.6721076965332031, 0.6531197428703308, 0.6446771025657654, 0.6552738547325134, 0.6362810134887695, 0.6469258069992065, 0.6279329657554626, 0.6386252045631409, 0.6196341514587402, 0.6005377769470215, 0.611386239528656, 0.5922955870628357, 0.6031908988952637, 0.5841078758239746, 0.5950496792793274, 0.575976550579071, 0.5869641900062561, 0.567903459072113, 0.5789360404014587, 0.5598904490470886, 0.5709671378135681, 0.5519396066665649, 0.5630595684051514, 0.5328202247619629, 0.5440531373023987, 0.5249583125114441, 0.5362334251403809, 0.5474361181259155, 0.5171664953231812, 0.5284825563430786, 0.5094473361968994, 0.5208029747009277, 0.5320833325386047, 0.5018035769462585, 0.5131973028182983, 0.5366259217262268, 0.494238018989563, 0.5170202255249023, 0.5417959690093994, 0.4867537021636963, 0.5096039175987244, 0.4677780270576477, 0.4908510148525238, 0.5148261785507202, 0.46043476462364197, 0.4835691750049591, 0.5202662944793701, 0.45318499207496643, 0.4648210406303406, 0.5132442712783813, 0.5743728280067444, 0.44603264331817627, 0.5188512206077576, 0.5290347337722778, 0.43898165225982666, 0.5370392799377441, 31.0,
s

---

Originally posted by rnunziata on ROS Answers with karma: 713 on 2015-03-21

Post score: 1

Answer 1

This is probably due to you not providing pointcloud_to_laserscan with a target frame. Camera frames are differently oriented (http://www.ros.org/reps/rep-0103.html#suffix-frames). Pointcloud_to_laserscan projects the pointcloud onto the x-y plane, so if the camera frame is used, the laserscan will end up 'vertical'.

---

Originally posted by paulbovbel with karma: 4518 on 2015-03-21

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by rnunziata on 2015-03-22:
The point cloud comes from the depth points from the gazebo kentic plugin which is converted to a scan line with a target frame of rrbot/camera_frame which is the same frame the riviz point cloud2 is using to display the voxel image. You can see both in the images.

Comment by paulbovbel on 2015-03-22:
Yes, and the camera_frame is oriented differently than you may expect (z-forward, x-right, y-up). pointcloud_to_laserscan will try to output a laserscan on the x-y plane, which is why it's 'upright'. Use a body-oriented frame (x-forward, y-left, z-up) such as 'base_link' as the target frame.

Comment by rnunziata on 2015-03-22:
OK...tried to reset the target frame in PtoS and get this error which I do not understand

Can't transform pointcloud from frame rrbot/camera_frame to camera_frame_optical  with tolerance 0.01

Comment by paulbovbel on 2015-03-22:
https://github.com/ros-perception/perception_pcl/blob/indigo-devel/pointcloud_to_laserscan%2Fsrc%2Fpointcloud_to_laserscan_nodelet.cpp#L131

This means there's a problem with your transform tree. Post the results of rqt_graph.

Comment by paulbovbel on 2015-03-22:
If you have a camera_frame and camera_frame_optical, then camera_frame should hypothetically be fine to use as a target_frame. You want to avoid using frames that use the _optical convention.

Comment by rnunziata on 2015-03-22:
I have updated the problem statement to include additional data.

Comment by paulbovbel on 2015-03-22:
Looks like you have a weird prefixing thing going on. Your pointclouds are coming in with frame "rrbot/camera_frame", but your tf tree contains "rrbot_camera_frame"

Comment by rnunziata on 2015-03-22:
I saw that but assumed it was how it handled the display was not an issue. I does find the frame and processes it with rotation on the static transform which is displayed correctly in rviz.

Comment by paulbovbel on 2015-03-22:
It looks like there's an issue with how you've setup your transforms/gazebo plugins. Take a look at https://github.com/husky/husky_simulator/blob/indigo-devel/husky_gazebo/urdf/accessories/kinect_camera.gazebo.xacro.

Comment by paulbovbel on 2015-03-22:
Your _optical frame should be rotated (-1.5708 0 -1.5708) relative to all its parent links, and your camera and kinect plugins should be publishing data in the optical frame.

Comment by paulbovbel on 2015-03-22:
Your pointcloud_to_laserscan node should have a target frame that's 'normal' as in x-forward, y-left, z-up, such as base_link,

Comment by rnunziata on 2015-03-23:
The plugin belongs to Gazebo ros. If I use the "rrbot/camera_frame" which is rotated as you suggest and produces the correct visual in rviz after rotation does not produce the correct laser output. Are you saying that this is a plugin issue?

Comment by paulbovbel on 2015-03-23:
No I'm saying you should a) correct your naming conventions, as _optical frames are the rotated frames, b) use base_link for your target frame, c) correct your tf naming issue as that may be causing the message filter in pcl_to_ls to fail

Comment by rnunziata on 2015-03-23:
I weather I use optical or not it does not make a difference since it is just a convention. I tried using base_link as you had suggested but it does not fix the issue. I am not using the (as yet to be determined) bad tf name since I am using the camera_frame_optical now which does not have this issu

Comment by rnunziata on 2015-03-23:
I believe there is something else going on and I am going to take this to pcl_to_laser issue board.

Comment by paulbovbel on 2015-03-23:
What frame are your pointclouds coming in on? Please check with rostopic echo.

Comment by rnunziata on 2015-03-23:
depth points are coming in on topic "/depth/points"/. If I leave off the target_frame then defaults to rrbot/camera_frame in the scan output but rotation is incorrect and or depth is not right.

Comment by paulbovbel on 2015-03-23:
The issue so far is pretty straightforward - even if by some miracle rviz manages to visualize your pointcloud, rrbot/camera_frame and rrbot_camera_frame are NOT equivalent frame_ids, and this will cause issues. Fix your transform tree, and then use base_link as your target_frame.

Comment by rnunziata on 2015-03-23:
rrbot_camera_frame is not a real frame , it does not show in tf, nor is it a valid link name. It is generated as rrbot/camera_frame by gazebo kenitic plugin. going to write my own pcl translation and force rotation.

Comment by paulbovbel on 2015-03-24:
It's the frame that shows up in your transform tree.