Skip to content

Commit

Permalink
Two more
Browse files Browse the repository at this point in the history
  • Loading branch information
Tobias-Fischer committed Apr 27, 2021
1 parent aa54eb0 commit b64388f
Show file tree
Hide file tree
Showing 3 changed files with 63 additions and 1 deletion.
25 changes: 25 additions & 0 deletions patch/ros-noetic-laser-ortho-projector.patch
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
diff --git a/src/laser_ortho_projector.cpp b/src/laser_ortho_projector.cpp
index c5bdc45..ae99129 100644
--- a/src/laser_ortho_projector.cpp
+++ b/src/laser_ortho_projector.cpp
@@ -191,7 +191,7 @@ void LaserOrthoProjector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& s
// **** build and publish projected cloud

PointCloudT::Ptr cloud =
- boost::shared_ptr<PointCloudT>(new PointCloudT());
+ std::shared_ptr<PointCloudT>(new PointCloudT());

pcl_conversions::toPCL(scan_msg->header, cloud->header);
cloud->header.frame_id = ortho_frame_;
@@ -217,7 +217,10 @@ void LaserOrthoProjector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& s
cloud->height = 1;
cloud->is_dense = true; // no nan's present

- cloud_publisher_.publish (cloud);
+ sensor_msgs::PointCloud2 out_cloud;
+ pcl::toROSMsg(*cloud, out_cloud);
+
+ cloud_publisher_.publish (out_cloud);
}

bool LaserOrthoProjector::getBaseToLaserTf (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
34 changes: 34 additions & 0 deletions patch/ros-noetic-scan-to-cloud-converter.patch
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
diff --git a/src/scan_to_cloud_converter.cpp b/src/scan_to_cloud_converter.cpp
index 749c9dd..8f19af4 100644
--- a/src/scan_to_cloud_converter.cpp
+++ b/src/scan_to_cloud_converter.cpp
@@ -43,7 +43,7 @@ ScanToCloudConverter::ScanToCloudConverter(ros::NodeHandle nh, ros::NodeHandle n
invalid_point_.y = std::numeric_limits<float>::quiet_NaN();
invalid_point_.z = std::numeric_limits<float>::quiet_NaN();

- cloud_publisher_ = nh_.advertise<PointCloudT>(
+ cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(
"cloud", 1);
scan_subscriber_ = nh_.subscribe(
"scan", 1, &ScanToCloudConverter::scanCallback, this);
@@ -57,7 +57,7 @@ ScanToCloudConverter::~ScanToCloudConverter()
void ScanToCloudConverter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
PointCloudT::Ptr cloud_msg =
- boost::shared_ptr<PointCloudT>(new PointCloudT());
+ std::shared_ptr<PointCloudT>(new PointCloudT());

cloud_msg->points.resize(scan_msg->ranges.size());

@@ -81,8 +81,10 @@ void ScanToCloudConverter::scanCallback(const sensor_msgs::LaserScan::ConstPtr&
cloud_msg->height = 1;
cloud_msg->is_dense = false; //contains nans
pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);
+ sensor_msgs::PointCloud2 out_cloud;
+ pcl::toROSMsg(*cloud_msg, out_cloud);

- cloud_publisher_.publish(cloud_msg);
+ cloud_publisher_.publish(out_cloud);
}

} //namespace scan_tools
5 changes: 4 additions & 1 deletion vinca_linux_64.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,10 @@ packages_select_by_deps:
##
# TODO Linux
##
- phidgets_drivers
- scan_to_cloud_converter
- laser_ortho_projector

# - phidgets_drivers
# - mrpt2
# - knowledge_representation
# - multirobot_map_merge
Expand Down

0 comments on commit b64388f

Please sign in to comment.