laser_assembler
laser_assembler copied to clipboard
Published
20 hours ago •
ros-perception
ros-perception
Published Cloud with 0 points
I am working with the laser_assembler package and sensor_msgs::PointCloud2 from a lidar. When i use the periodic_snapshotter (modified for pointcloud2 , as below) I get the no point clouds published on the /assembled_cloud topic. What could be the possible cause of no assembling of the cloud even though no error is received. I am using ROS Noetic on Ubuntu 20. Lidar source OS0-32
#include <cstdio>
#include <ros/ros.h>
// Services
#include "laser_assembler/AssembleScans2.h"
// Messages
#include "sensor_msgs/PointCloud2.h"
/***
* This a simple test app that requests a point cloud from the
* point_cloud_assembler every 4 seconds, and then publishes the
* resulting data
*/
namespace laser_assembler
{
class PeriodicSnapshotter
{
public:
PeriodicSnapshotter()
{
// Create a publisher for the clouds that we assemble
pub_ = n_.advertise<sensor_msgs::PointCloud2> ("assembled_cloud", 1);
// Create the service client for calling the assembler
client_ = n_.serviceClient<AssembleScans2>("assemble_scans2");
// Start the timer that will trigger the processing loop (timerCallback)
timer_ = n_.createTimer(ros::Duration(2,0), &PeriodicSnapshotter::timerCallback, this);
// Need to track if we've called the timerCallback at least once
first_time_ = true;
}
void timerCallback(const ros::TimerEvent& e)
{
// We don't want to build a cloud the first callback, since we
// don't have a start and end time yet
if (first_time_)
{
first_time_ = false;
return;
}
// Populate our service request based on our timer callback times
AssembleScans2 srv;
srv.request.begin = e.last_real;
srv.request.end = e.current_real;
// Make the service call
if (client_.call(srv))
{
ROS_INFO("Published Cloud with %u points", (uint32_t)(srv.response.cloud.data.size())) ;
pub_.publish(srv.response.cloud);
}
else
{
ROS_ERROR("Error making service call\n") ;
}
}
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::ServiceClient client_;
ros::Timer timer_;
bool first_time_;
} ;
}
using namespace laser_assembler ;
int main(int argc, char **argv)
{
ros::init(argc, argv, "periodic_snapshotter");
ros::NodeHandle n;
ROS_INFO("Waiting for [build_cloud] to be advertised");
ros::service::waitForService("build_cloud2");
ROS_INFO("Found build_cloud! Starting the snapshotter");
PeriodicSnapshotter snapshotter;
ros::spin();
return 0;
}
You advertise on a relative topic (no leading slash). So I'd expect the topic to be periodic_snapshotter/assembled_cloud not /assembled_cloud, right?
Are you seeing the "ublished Cloud with %u points" message in the console?
Yes, I see the message. The output is something like:
[ INFO] [1696281996.306222694]: Waiting for [build_cloud] to be advertised
[ INFO] [1696281996.307338430]: Found build_cloud! Starting the snapshotter
[ INFO] [1696282000.310993023]: Published Cloud with 0 points
[ INFO] [1696282002.310856035]: Published Cloud with 0 points
[ INFO] [1696282004.310881261]: Published Cloud with 0 points
[ INFO] [1696282006.310847268]: Published Cloud with 0 points
[ INFO] [1696282008.310983764]: Published Cloud with 0 points
[ INFO] [1696282010.310849034]: Published Cloud with 0 points
[ INFO] [1696282012.310866551]: Published Cloud with 0 points
[ INFO] [1696282014.310888687]: Published Cloud with 0 points
[ INFO] [1696282016.310844742]: Published Cloud with 0 points
[ INFO] [1696282018.310878123]: Published Cloud with 0 points
[ INFO] [1696282020.310872522]:
My launch file looks like below:
<launch>
<node type="point_cloud2_assembler" pkg="laser_assembler"
name="my_assembler">
<remap from="cloud" to="/ouster/points"/>
<param name="max_clouds" type="int" value="400" />
<param name="fixed_frame" type="string" value="base_link" />
</node>
<node pkg="rosbag" type="play" name="rosbag_player"
output="screen" args="--clock /home/pandey/xx/src/rosbags/rosbags/15-inforward.bag"/>
<node type="vslam_tf_broadcaster.py" pkg="vslam_tf"
name="vslam_tf">
</node>
</launch>
I am using a rosbag to get the pointcloud data and I have tf broadcaster node to publish the tf (translation ans rotation)
Do you have the use_sim_time parameter set to true? Otherwise all ROS nodes will use the system clock, instead of the published times from rosbag: http://wiki.ros.org/Clock#Using_Simulation_Time_from_the_.2BAC8-clock_Topic
Yes, I have set the <param name="/use_sim_time" value="true" /> through the launch file, but sadly I am getting the same response. Also, I am receiving the following warning
[ WARN] [1696285600.074118357, 1696015196.051577037]: MessageFilter [target=base_link ]: Dropped 95.65% of messages so far. Please turn the [ros.laser_assembler.message_filter] rosconsole logger to DEBUG for more information.
I tried checking the rosconsole in debug mode but I have n't received any useful informaton. Adding the DEBUG messages:
[DEBUG] [1696286386.933006375, 1696015209.178309991]: Connection::drop(2)
[DEBUG] [1696286386.933054915, 1696015209.178309991]: TCP socket [10] closed
[DEBUG] [1696286386.933081872, 1696015209.178309991]: Connection::drop(0)
[DEBUG] [1696286386.933102658, 1696015209.178309991]: Connection::drop(2)
[DEBUG] [1696286386.933139151, 1696015209.178309991]: Connection::drop(2)
[DEBUG] [1696286387.873783604, 1696015210.117482810]: Resolved publisher host [pandey] to [127.0.1.1] for socket [10]
[DEBUG] [1696286387.873882289, 1696015210.117482810]: Adding tcp socket [10] to pollset
[DEBUG] [1696286387.873917754, 1696015210.117482810]: Async connect() in progress to [pandey:36493] on socket [10]
[DEBUG] [1696286387.873959609, 1696015210.117482810]: Async socket[10] is connected
[DEBUG] [1696286387.874591572, 1696015210.117482810]: Connection::drop(2)
[DEBUG] [1696286387.874669798, 1696015210.117482810]: TCP socket [10] closed
[DEBUG] [1696286387.874694920, 1696015210.117482810]: Connection::drop(0)
[ INFO] [1696286387.874725680, 1696015210.117482810]: Published Cloud with 0 points
[DEBUG] [1696286387.874756623, 1696015210.117482810]: Trying to publish message of type [sensor_msgs/PointCloud2/1158d486dd51d683ce2f1be655c3c181] on a publisher with type [sensor_msgs/PointCloud2/1158d486dd51d683ce2f1be655c3c181]
[DEBUG] [1696286387.874794146, 1696015210.117482810]: Service client from [callerid=[/my_assembler] address=[TCPROS connection on port 53164 to [pandey:36493 on socket 10]]] for [/assemble_scans2] dropped
[DEBUG] [1696286387.874829828, 1696015210.117482810]: Connection::drop(2)
[DEBUG] [1696286389.875634147, 1696015212.119124327]: Resolved publisher host [pandey] to [127.0.1.1] for socket [10]
[DEBUG] [1696286389.875827944, 1696015212.119124327]: Adding tcp socket [10] to pollset
[DEBUG] [1696286389.875903023, 1696015212.119124327]: Async connect() in progress to [pandey:36493] on socket [10]
[DEBUG] [1696286389.875979027, 1696015212.119124327]: Async socket[10] is connected
[DEBUG] [1696286389.877142110, 1696015212.119124327]: Connection::drop(2)
[ INFO] [1696286389.877290430, 1696015212.119124327]: Published Cloud with 0 points
[DEBUG] [1696286389.877384809, 1696015212.119124327]: TCP socket [10] closed
[DEBUG] [1696286389.877442048, 1696015212.119124327]: Connection::drop(0)
[DEBUG] [1696286389.877525366, 1696015212.119124327]: Service client from [callerid=[/my_assembler] address=[TCPROS connection on port 53180 to [pandey:36493 on socket 10]]] for [/assemble_scans2] dropped
[DEBUG] [1696286389.877605221, 1696015212.119124327]: Connection::drop(2)
[DEBUG] [1696286391.876126190, 1696015214.120005022]: Resolved publisher host [pandey] to [127.0.1.1] for socket [10]
[DEBUG] [1696286391.876226986, 1696015214.120005022]: Adding tcp socket [10] to pollset
[DEBUG] [1696286391.876261110, 1696015214.120005022]: Async connect() in progress to [pandey:36493] on socket [10]
[DEBUG] [1696286391.876300553, 1696015214.120005022]: Async socket[10] is connected
[DEBUG] [1696286391.876972070, 1696015214.120005022]: Connection::drop(2)
[ INFO] [1696286391.877055097, 1696015214.120005022]: Published Cloud with 0 points
[DEBUG] [1696286391.877107149, 1696015214.120005022]: TCP socket [10] closed
[DEBUG] [1696286391.877137602, 1696015214.120005022]: Connection::drop(0)
[DEBUG] [1696286391.877165533, 1696015214.120005022]: Service client from [callerid=[/my_assembler] address=[TCPROS connection on port 53196 to [pandey:36493 on socket 10]]] for [/assemble_scans2] dropped
[DEBUG] [1696286391.877211759, 1696015214.120005022]: Connection::drop(2)
[DEBUG] [1696286393.867676375, 1696015216.111372530]: Resolved publisher host [pandey] to [127.0.1.1] for socket [10]
[DEBUG] [1696286393.867751051, 1696015216.111372530]: Adding tcp socket [10] to pollset
[DEBUG] [1696286393.867777518, 1696015216.111372530]: Async connect() in progress to [pandey:36493] on socket [10]
[DEBUG] [1696286393.867803140, 1696015216.111372530]: Async socket[10] is connected
[DEBUG] [1696286393.868449585, 1696015216.111372530]: Connection::drop(2)
[ INFO] [1696286393.868512141, 1696015216.111372530]: Published Cloud with 0 points
[DEBUG] [1696286393.868562216, 1696015216.111372530]: TCP socket [10] closed
[DEBUG] [1696286393.868590484, 1696015216.111372530]: Connection::drop(0)
[DEBUG] [1696286393.868612343, 1696015216.111372530]: Service client from [callerid=[/my_assembler] address=[TCPROS connection on port 33128 to [pandey:36493 on socket 10]]] for [/assemble_scans2] dropped
[DEBUG] [1696286393.868659173, 1696015216.111372530]: Connection::drop(2)
[DEBUG] [1696286395.868000072, 1696015218.112378716]: Resolved publisher host [pandey] to [127.0.1.1] for socket [10]
[DEBUG] [1696286395.868099760, 1696015218.112378716]: Adding tcp socket [10] to pollset
[DEBUG] [1696286395.868134156, 1696015218.112378716]: Async connect() in progress to [pandey:36493] on socket [10]
[DEBUG] [1696286395.868167458, 1696015218.112378716]: Async socket[10] is connected
[DEBUG] [1696286395.868829674, 1696015218.112378716]: Connection::drop(2)
[ INFO] [1696286395.868876273, 1696015218.112378716]: Published Cloud with 0 points
[DEBUG] [1696286395.868926091, 1696015218.112378716]: TCP socket [10] closed
[DEBUG] [1696286395.868952951, 1696015218.112378716]: Connection::drop(0)
[DEBUG] [1696286395.868979209, 1696015218.112378716]: Service client from [callerid=[/my_assembler] address=[TCPROS connection on port 33132 to [pandey:36493 on socket 10]]] for [/assemble_scans2] dropped
[DEBUG] [1696286395.869016147, 1696015218.112378716]: Connection::drop(2)
[DEBUG] [1696286397.021909789, 1696015219.001803548]: Socket [12] received 0/4 bytes, closing
[DEBUG] [1696286397.022108813, 1696015219.001803548]: TCP socket [12] closed
[DEBUG] [1696286397.022171121, 1696015219.001803548]: Connection::drop(0)
[DEBUG] [1696286397.022242246, 1696015219.001803548]: Connection to publisher [TCPROS connection on port 56300 to [pandey:51345 on socket 12]] to topic [/clock] dropped
[DEBUG] [1696286397.022445608, 1696015219.001803548]: getsockopt failed on socket [-1]
[DEBUG] [1696286397.022517799, 1696015219.001803548]: Socket -1 closed with (ERR|HUP|NVAL) events 8193: Unknown error -1
[DEBUG] [1696286397.045900945, 1696015219.001803548]: Received update for topic [/clock] (0 publishers)
[DEBUG] [1696286397.045989557, 1696015219.001803548]: Publisher update for [/clock]: already have these connections: http://pandey:40443/,
[DEBUG] [1696286397.046068244, 1696015219.001803548]: Disconnecting from publisher [/rosbag_player] of topic [/clock] at [http://pandey:40443/]
[DEBUG] [1696286397.046115047, 1696015219.001803548]: Connection::drop(2)
[DEBUG] [1696286397.046165352, 1696015219.001803548]: Connection::drop(2)
[DEBUG] [1696286397.046206661, 1696015219.001803548]: Connection::drop(2)
Can you check the coordinate frame that the laser messages are published in, and then run, rosrun tf tf_echo base_link <laser_frame>.
So, I received the following tf:
Failure at 1696015181.756943319
Exception thrown:"base_link" passed to lookupTransform argument target_frame does not exist.
The current list of frames is:
Frame os_lidar exists with parent os_sensor.
Frame os_imu exists with parent os_sensor.
Failure at 1696015181.756943319
Exception thrown:"base_link" passed to lookupTransform argument target_frame does not exist.
The current list of frames is:
Frame os_lidar exists with parent os_sensor.
Frame os_imu exists with parent os_sensor.
At time 1696015181.774
- Translation: [-0.005, 0.001, -0.036]
- Rotation: in Quaternion [0.000, -0.002, -0.002, 1.000]
in RPY (radian) [0.000, -0.004, -0.005]
in RPY (degree) [0.006, -0.241, -0.285]
At time 1696015182.108
- Translation: [-0.010, 0.004, -0.074]
- Rotation: in Quaternion [-0.000, -0.006, -0.005, 1.000]
in RPY (radian) [-0.000, -0.013, -0.009]
in RPY (degree) [-0.024, -0.733, -0.541]
At time 1696015182.440
- Translation: [-0.012, 0.005, -0.103]
- Rotation: in Quaternion [-0.000, -0.008, -0.008, 1.000]
in RPY (radian) [-0.001, -0.016, -0.017]
in RPY (degree) [-0.035, -0.937, -0.959]
At time 1696015182.777
- Translation: [-0.010, 0.006, -0.131]
- Rotation: in Quaternion [-0.000, -0.006, -0.011, 1.000]
in RPY (radian) [-0.000, -0.011, -0.023]
in RPY (degree) [-0.015, -0.636, -1.306]
At time 1696015183.108
- Translation: [-0.010, 0.008, -0.161]
- Rotation: in Quaternion [-0.001, -0.004, -0.014, 1.000]
in RPY (radian) [-0.001, -0.008, -0.028]
in RPY (degree) [-0.075, -0.431, -1.585]
At time 1696015183.440
- Translation: [-0.010, 0.011, -0.194]
- Rotation: in Quaternion [-0.002, -0.004, -0.017, 1.000]
in RPY (radian) [-0.004, -0.009, -0.034]
in RPY (degree) [-0.229, -0.499, -1.940]
At time 1696015183.777
- Translation: [-0.012, 0.011, -0.230]
- Rotation: in Quaternion [-0.001, -0.005, -0.020, 1.000]
in RPY (radian) [-0.001, -0.011, -0.039]
in RPY (degree) [-0.079, -0.616, -2.252]
At time 1696015184.108
- Translation: [-0.012, 0.014, -0.275]
- Rotation: in Quaternion [-0.000, -0.008, -0.025, 1.000]
in RPY (radian) [0.000, -0.017, -0.050]
in RPY (degree) [0.010, -0.973, -2.851]
At time 1696015184.444
- Translation: [-0.014, 0.015, -0.318]
- Rotation: in Quaternion [-0.001, -0.011, -0.030, 0.999]
in RPY (radian) [-0.002, -0.023, -0.060]
in RPY (degree) [-0.122, -1.316, -3.447]
At time 1696015184.779
- Translation: [-0.014, 0.018, -0.353]
- Rotation: in Quaternion [-0.000, -0.013, -0.035, 0.999]
in RPY (radian) [0.000, -0.025, -0.071]
in RPY (degree) [0.028, -1.437, -4.048]
At time 1696015185.112
- Translation: [-0.013, 0.019, -0.383]
- Rotation: in Quaternion [-0.000, -0.012, -0.040, 0.999]
in RPY (radian) [0.000, -0.023, -0.080]
in RPY (degree) [0.028, -1.335, -4.593]
At time 1696015185.443
- Translation: [-0.013, 0.021, -0.402]
- Rotation: in Quaternion [-0.001, -0.010, -0.042, 0.999]
in RPY (radian) [-0.001, -0.020, -0.084]
in RPY (degree) [-0.056, -1.128, -4.836]
So, I found that the createTime function was causing all the problem. For some reason it keeps spitting 0 (zero) for e.last_real and e.current_real - I have not been able to diagnose why it is happening.
Can you rostopic echo /clock to make sure that rosbag is publishing it correctly?