publish sonar to ROS

This commit is contained in:
Wojciech Kwolek 2021-04-21 09:32:05 +00:00
parent 4f2a9046df
commit 26e182a855
4 changed files with 16 additions and 1 deletions

View File

@ -5,4 +5,5 @@ void Ros2Aria::publish()
// RCLCPP_INFO(this->get_logger(), "publish");
rclcpp::Time t = robot->getClock()->now();
sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
publishSonar(sonarData);
}

View File

@ -14,6 +14,8 @@ Ros2Aria::Ros2Aria()
"cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
// listen to sensors
auto r = robot->getRobot();
r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);

View File

@ -23,7 +23,10 @@ private:
// publishers
void publish();
rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr sonar_pub_;
sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
void publishSonar(sensor_msgs::msg::PointCloud cloud);
// subscribers
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

View File

@ -2,7 +2,6 @@
sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
{
sensor_msgs::msg::PointCloud cloud; //sonar readings.
cloud.header.stamp = stamp;
// sonar sensors relative to base_link
@ -42,3 +41,13 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
}
return cloud;
}
void Ros2Aria::publishSonar(sensor_msgs::msg::PointCloud cloud)
{
if (this->sonar_pub_->get_subscription_count() == 0)
return;
RCLCPP_INFO(this->get_logger(), "publishing sonar pointcloud");
this->sonar_pub_->publish(cloud);
}