L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

publish sonar to ROS

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/ros2aria/src/publish.cpp
View File

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

2
src/ros2aria/src/ros2aria.cpp
View File

@ -14,6 +14,8 @@ Ros2Aria::Ros2Aria()
"cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1)); "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)); 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 // listen to sensors
auto r = robot->getRobot(); auto r = robot->getRobot();
r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb); r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);

3
src/ros2aria/src/ros2aria.hpp
View File

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

11
src/ros2aria/src/sonar.cpp
View File

@ -2,7 +2,6 @@
sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp) sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
{ {
sensor_msgs::msg::PointCloud cloud; //sonar readings. sensor_msgs::msg::PointCloud cloud; //sonar readings.
cloud.header.stamp = stamp; cloud.header.stamp = stamp;
// sonar sensors relative to base_link // sonar sensors relative to base_link
@ -42,3 +41,13 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
} }
return cloud; 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);
}