45 #include <sensor_msgs/PointCloud.h> 49 #include <boost/thread/mutex.hpp> 50 #include <boost/shared_ptr.hpp> 51 #include <boost/bind.hpp> 68 if (output_frame_.empty())
69 ROS_ERROR(
"No output frame specified for merging pointclouds");
72 if (max_freq_ > 1000.0 || max_freq_ < 0.0)
111 sensor_msgs::PointCloud out;
120 std::copy(
cloud1_.points.begin(),
cloud1_.points.end(), out.points.begin());
124 for (
unsigned int i = 0 ; i <
cloud1_.channels.size() ; ++i)
125 for (
unsigned int j = 0 ; j <
cloud2_.channels.size() ; ++j)
130 unsigned int oc = out.channels.size();
131 out.channels.resize(oc + 1);
132 out.channels[oc].name =
cloud1_.channels[i].name;
133 out.channels[oc].values.resize(
cloud1_.channels[i].values.size() +
cloud2_.channels[j].values.size());
134 std::copy(
cloud1_.channels[i].values.begin(),
cloud1_.channels[i].values.end(), out.channels[oc].values.begin());
135 std::copy(
cloud2_.channels[j].values.begin(),
cloud2_.channels[j].values.end(), out.channels[oc].values.begin() +
cloud1_.channels[i].values.size());
165 void processCloud(
const sensor_msgs::PointCloudConstPtr &cloud, sensor_msgs::PointCloud &cloudOut)
198 int main(
int argc,
char **argv)
202 ROS_WARN(
"laser_assembler/merge_clouds is deprecated. You should instead be using 3dnav_pr2/merge_clouds");
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::PointCloud cloud2_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::PointCloud > > tf_filter2_
ROSCPP_DECL void spin(Spinner &spinner)
void processCloud(const sensor_msgs::PointCloudConstPtr &cloud, sensor_msgs::PointCloud &cloudOut)
std::string output_frame_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void receiveCloud1(const sensor_msgs::PointCloudConstPtr &cloud)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf::TransformListener tf_
void receiveCloud2(const sensor_msgs::PointCloudConstPtr &cloud)
sensor_msgs::PointCloud cloud1_
int main(int argc, char **argv)
message_filters::Subscriber< sensor_msgs::PointCloud > sub2_
void onTimer(const ros::TimerEvent &e)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::PointCloud > > tf_filter1_
message_filters::Subscriber< sensor_msgs::PointCloud > sub1_