publish sonar to ROS
This commit is contained in:
parent
4f2a9046df
commit
26e182a855
@ -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);
|
||||
}
|
@ -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);
|
||||
|
@ -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_;
|
||||
|
@ -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);
|
||||
}
|
Loading…
Reference in New Issue
Block a user