46 sensor_msgs::JointStatePtr
first_state1(
new sensor_msgs::JointState);
47 sensor_msgs::JointStatePtr
first_state2(
new sensor_msgs::JointState);
54 void state_cb (
const sensor_msgs::JointState::ConstPtr& state_in)
57 std::string joint_name =
"/connect_laser_roof_rotating";
58 if (joint_name.compare(state_in->name[0]) != 0)
72 sensor_msgs::JointStatePtr new_state(
new sensor_msgs::JointState);
76 g_las3D->
joints.push_back(new_state);
85 void scan_cb(
const sensor_msgs::LaserScan::ConstPtr& scan_in)
90 sensor_msgs::LaserScanPtr new_scan(
new sensor_msgs::LaserScan);
93 sensor_msgs::PointCloud2 pcmsg_in;
101 pcl::PointCloud<pcl::PointXYZ> pc_in;
102 pcl::PCLPointCloud2 pcl_pc;
103 pcl_conversions::toPCL(pcmsg_in, pcl_pc);
104 pcl::fromPCLPointCloud2(pcl_pc, pc_in);
106 pcl::fromROSMsg(pcmsg_in,pc_in);
136 sensor_msgs::LaserScanPtr new_scan(
new sensor_msgs::LaserScan);
139 g_las3D->
scans.push_back(new_scan);
141 if(g_las3D->
joints.size()<2)
144 while(g_las3D->
joints.size()>=2 && g_las3D->
scans.size()>0)
147 sensor_msgs::LaserScanPtr working_scan(
new sensor_msgs::LaserScan);
148 *working_scan=*g_las3D->
scans[0];
152 sensor_msgs::JointStatePtr state_start(
new sensor_msgs::JointState);
153 *state_start=*g_las3D->
joints[0];
158 sensor_msgs::JointStatePtr state_end(
new sensor_msgs::JointState);
159 *state_end=*g_las3D->
joints[0];
162 while (working_scan->header.stamp<state_start->header.stamp)
164 if (g_las3D->
scans.size()>0)
165 *working_scan=*g_las3D->
scans[0];
176 while (working_scan->header.stamp>state_end->header.stamp)
180 if (g_las3D->
joints.size()>0)
181 *state_end=*g_las3D->
joints[0];
192 sensor_msgs::PointCloud2 pcmsg_in;
201 pcl::PointCloud<pcl::PointXYZ> pc_in;
202 pcl::fromROSMsg(pcmsg_in,pc_in);
218 int main(
int argc,
char **argv)
220 ros::init(argc, argv,
"laser3D_pointcloud");
221 ros::NodeHandle n(
"~");
222 tf::TransformListener listener(n,ros::Duration(100));
230 n.param(
"output_frequency", output_freq, 200.0);
236 if (ros::names::remap(
"/ac_frame")==
"/ac_frame")
238 ROS_ERROR(
"/ac_frame was not remapped. Aborting program.");
243 ros::Subscriber sub_phiversion = n.subscribe (
"/laserscan0", 100,
scan_cb);
244 ros::Subscriber sub_state = n.subscribe (
"/joint_state", 100,
state_cb);
247 ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>(
"/pc_out", 1);
249 ros::Rate loop_rate(output_freq);
258 sensor_msgs::PointCloud2 pcmsg_out;
262 pcmsg_out.header.stamp = ros::Time::now();
265 std::cout<<
"The message stamp is missing!!, please solve!!"<<std::endl;
269 ROS_INFO(
"Publishing pc_accumulated with %d points", (
int)g_las3D->
pc_accumulated.size());
272 pub.publish(pcmsg_out);
void scan_cb(const sensor_msgs::LaserScan::ConstPtr &scan_in)
Callback from the LaserScan subscribed topic.
int accumulation_mode
Type of accumulation wanted to be used.
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
PointCloud<pcl::PointXYZ> to accumulate the laser scans.
int laser_count
Number of laser scans received.
int main(int argc, char **argv)
void state_cb(const sensor_msgs::JointState::ConstPtr &state_in)
Callback from the JointState subscribed topic.
int pointcloud_stamp
Time stamp to the output cloud, when using rosbag or in real time accumulation.
ros::Time firstscan_stamp
std::vector< sensor_msgs::JointStatePtr > joints
Vector with the external shaft angle mensage.
void accumulate_cloud(pcl::PointCloud< pcl::PointXYZ > *pc_in)
Function to accumulate PointCloud. Receives the pointcloud with the transformed laser scan and accumu...
sensor_msgs::JointStatePtr first_state1(new sensor_msgs::JointState)
las3D_PointCloud * g_las3D
sensor_msgs::JointStatePtr first_state2(new sensor_msgs::JointState)
tf::TransformListener * p_listener
ROS tf listener.
std::vector< sensor_msgs::LaserScanPtr > scans
Vector with the laser scans received.
Class to accumulate point clouds laser3D.
bool laserscan_arrived
bool laser scan received
size_t cloud_size
PointCloud size.
int max_scans_accumulated
Max of scans to be accumulated.
int las3D_transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, const sensor_msgs::JointState &state_start, const sensor_msgs::JointState &state_end, tf::Transformer &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame using the external...