I am working on collision prevention in px4 (v1.14) VIO based drone using YDLidar T-mini Pro 360 deg Lidar (2D).
The setup includes:
Cubepilot cube orange plus.
Nvidia issac ros for realsense based VIO running on jetson orin nano DK.
Intel realsense D435i
I am using px4_msgs (obstacle_dostance) for conversion of “/scan” (LaserScan) data from ros2 to px4 using microxrce_dds.
I am getting values in “/fmu/in/obstacle_distance” uorb topic created by xrce_dds, but the distances are not visible and usable in px4 (param CP_DIST=1 enabled).
Here is the script (C++) for ros2 node:
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "px4_msgs/msg/obstacle_distance.hpp"
class LaserScanToObstacleDistanceNode : public rclcpp::Node
{
public:
LaserScanToObstacleDistanceNode()
: Node("laserscan_to_obstacle_distance_node")
{
auto qos = rclcpp::QoS(20); // History depth is 10
qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
publisher_ = this->create_publisher<px4_msgs::msg::ObstacleDistance>("/fmu/in/obstacle_distance", qos);
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"/scan", qos, std::bind(&LaserScanToObstacleDistanceNode::listener_callback, this, std::placeholders::_1));
}
private:
void listener_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
auto obstacle_distance = laserscan_to_obstacle_distance(msg);
publisher_->publish(obstacle_distance);
}
px4_msgs::msg::ObstacleDistance laserscan_to_obstacle_distance(const sensor_msgs::msg::LaserScan::SharedPtr scan)
{
px4_msgs::msg::ObstacleDistance obstacle_distance;
obstacle_distance.timestamp = scan->header.stamp.sec * 1000000 + scan->header.stamp.nanosec / 1000; // Convert to microseconds
obstacle_distance.frame = 12;
obstacle_distance.sensor_type = 0;
// obstacle_distance.increment = scan->angle_increment;
obstacle_distance.increment = 5.0;
obstacle_distance.min_distance = scan->range_min * 100; // Convert to centimeters
obstacle_distance.max_distance = scan->range_max * 100; // Convert to centimeters
obstacle_distance.angle_offset = 0.0; // Assuming the lidar is mounted in the front
for (size_t i = 0; i < scan->ranges.size() && i < obstacle_distance.distances.size(); ++i)
{
obstacle_distance.distances[i] = std::min(std::max(static_cast<int>(scan->ranges[i] * 100), static_cast<int>(obstacle_distance.min_distance)), static_cast<int>(obstacle_distance.max_distance)); // Convert to centimeters
}
// Print the total length of the final array with distances
RCLCPP_INFO(this->get_logger(), "Total length of the final array with distances: %zu", obstacle_distance.distances.size());
return obstacle_distance;
}
rclcpp::Publisher<px4_msgs::msg::ObstacleDistance>::SharedPtr publisher_;
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LaserScanToObstacleDistanceNode>());
rclcpp::shutdown();
return 0;
}
Please help me with this.
Thank You !!