test costmap + cam depth
This commit is contained in:
461
src/depth_local_costmap_noetic_test_node.cpp
Normal file
461
src/depth_local_costmap_noetic_test_node.cpp
Normal file
@@ -0,0 +1,461 @@
|
||||
#include <robot_costmap_2d/costmap_2d.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/layer.h>
|
||||
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <robot_depth_image_proc/ros_message_conversions.h>
|
||||
#include <robot/robot.h>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_sensor_msgs/CameraInfo.h>
|
||||
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||
#include <robot_sensor_msgs/Image.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/PointField.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <tf2/exceptions.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <exception>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace
|
||||
{
|
||||
robot_sensor_msgs::PointCloud2 toRobotPointCloud2(const sensor_msgs::PointCloud2& msg)
|
||||
{
|
||||
robot_sensor_msgs::PointCloud2 robot_pc;
|
||||
|
||||
robot_pc.header.seq = msg.header.seq;
|
||||
robot_pc.header.stamp = robot::Time(msg.header.stamp.toSec());
|
||||
robot_pc.header.frame_id = msg.header.frame_id;
|
||||
robot_pc.height = msg.height;
|
||||
robot_pc.width = msg.width;
|
||||
|
||||
robot_pc.fields.clear();
|
||||
robot_pc.fields.reserve(msg.fields.size());
|
||||
for (const auto& field : msg.fields)
|
||||
{
|
||||
robot_sensor_msgs::PointField robot_field;
|
||||
robot_field.name = field.name;
|
||||
robot_field.offset = field.offset;
|
||||
robot_field.datatype = field.datatype;
|
||||
robot_field.count = field.count;
|
||||
robot_pc.fields.push_back(robot_field);
|
||||
}
|
||||
|
||||
robot_pc.is_bigendian = msg.is_bigendian;
|
||||
robot_pc.point_step = msg.point_step;
|
||||
robot_pc.row_step = msg.row_step;
|
||||
robot_pc.is_dense = msg.is_dense;
|
||||
robot_pc.data = msg.data;
|
||||
return robot_pc;
|
||||
}
|
||||
|
||||
bool isVoxelLayer(const robot_costmap_2d::Layer& layer)
|
||||
{
|
||||
return layer.getType() == robot_costmap_2d::LayerType::VOXEL_LAYER ||
|
||||
layer.getName() == "local_costmap/voxel_layer";
|
||||
}
|
||||
|
||||
template <typename MessageT>
|
||||
void feedVoxelLayers(robot_costmap_2d::Costmap2DROBOT& local_costmap,
|
||||
const MessageT& message,
|
||||
const std::string& topic)
|
||||
{
|
||||
if (local_costmap.getLayeredCostmap() == nullptr ||
|
||||
local_costmap.getLayeredCostmap()->getPlugins() == nullptr)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (const auto& layer : *local_costmap.getLayeredCostmap()->getPlugins())
|
||||
{
|
||||
if (layer && isVoxelLayer(*layer))
|
||||
layer->dataCallBack<MessageT>(message, topic);
|
||||
}
|
||||
}
|
||||
|
||||
class RosTfToTf3Bridge
|
||||
{
|
||||
public:
|
||||
RosTfToTf3Bridge(std::shared_ptr<tf2_ros::Buffer> tf2_buffer, tf3::BufferCore& tf3_buffer)
|
||||
: tf2_buffer_(std::move(tf2_buffer))
|
||||
, tf3_buffer_(tf3_buffer)
|
||||
, worker_(&RosTfToTf3Bridge::run, this)
|
||||
{
|
||||
tf3_buffer_.setUsingDedicatedThread(true);
|
||||
}
|
||||
|
||||
~RosTfToTf3Bridge()
|
||||
{
|
||||
stop_ = true;
|
||||
if (worker_.joinable())
|
||||
worker_.join();
|
||||
}
|
||||
|
||||
private:
|
||||
struct TfEdge
|
||||
{
|
||||
std::string parent;
|
||||
std::string child;
|
||||
};
|
||||
|
||||
static std::vector<TfEdge> parseEdges(const std::string& tree)
|
||||
{
|
||||
std::vector<TfEdge> edges;
|
||||
std::istringstream stream(tree);
|
||||
std::string line;
|
||||
|
||||
while (std::getline(stream, line))
|
||||
{
|
||||
const std::size_t frame_pos = line.find("Frame ");
|
||||
const std::size_t parent_pos = line.find(" exists with parent ");
|
||||
if (frame_pos == std::string::npos || parent_pos == std::string::npos)
|
||||
continue;
|
||||
|
||||
std::string child = line.substr(frame_pos + 6, parent_pos - (frame_pos + 6));
|
||||
std::string parent = line.substr(parent_pos + 20);
|
||||
if (!parent.empty() && parent.back() == '.')
|
||||
parent.pop_back();
|
||||
if (!parent.empty() && !child.empty())
|
||||
edges.push_back({parent, child});
|
||||
}
|
||||
|
||||
return edges;
|
||||
}
|
||||
|
||||
void run()
|
||||
{
|
||||
ros::Rate rate(50.0);
|
||||
|
||||
while (ros::ok() && !stop_)
|
||||
{
|
||||
if (!tf2_buffer_)
|
||||
{
|
||||
rate.sleep();
|
||||
continue;
|
||||
}
|
||||
|
||||
const std::vector<TfEdge> edges = parseEdges(tf2_buffer_->allFramesAsString());
|
||||
for (const auto& edge : edges)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!tf2_buffer_->canTransform(edge.parent, edge.child, ros::Time(0), ros::Duration(0.01)))
|
||||
continue;
|
||||
|
||||
const geometry_msgs::TransformStamped ros_tf =
|
||||
tf2_buffer_->lookupTransform(edge.parent, edge.child, ros::Time(0));
|
||||
|
||||
tf3::TransformStampedMsg tf3_msg;
|
||||
tf3_msg.header.stamp = tf3::Time::now();
|
||||
tf3_msg.header.frame_id = ros_tf.header.frame_id;
|
||||
tf3_msg.child_frame_id = ros_tf.child_frame_id;
|
||||
tf3_msg.transform.translation.x = ros_tf.transform.translation.x;
|
||||
tf3_msg.transform.translation.y = ros_tf.transform.translation.y;
|
||||
tf3_msg.transform.translation.z = ros_tf.transform.translation.z;
|
||||
tf3_msg.transform.rotation.x = ros_tf.transform.rotation.x;
|
||||
tf3_msg.transform.rotation.y = ros_tf.transform.rotation.y;
|
||||
tf3_msg.transform.rotation.z = ros_tf.transform.rotation.z;
|
||||
tf3_msg.transform.rotation.w = ros_tf.transform.rotation.w;
|
||||
tf3_buffer_.setTransform(tf3_msg, "ros_tf_bridge");
|
||||
}
|
||||
catch (const tf2::TransformException& ex)
|
||||
{
|
||||
ROS_WARN_THROTTLE(5.0, "TF bridge failed %s -> %s: %s",
|
||||
edge.parent.c_str(),
|
||||
edge.child.c_str(),
|
||||
ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
|
||||
tf3::BufferCore& tf3_buffer_;
|
||||
std::atomic<bool> stop_{false};
|
||||
std::thread worker_;
|
||||
};
|
||||
|
||||
class OdomTfBridge
|
||||
{
|
||||
public:
|
||||
OdomTfBridge(ros::NodeHandle& private_nh, const std::string& base_frame)
|
||||
: base_frame_(base_frame)
|
||||
{
|
||||
private_nh.param("odom_topic", odom_topic_, std::string("/odom"));
|
||||
odom_sub_ = private_nh.subscribe(odom_topic_, 10, &OdomTfBridge::odomCallback, this);
|
||||
ROS_WARN("Publishing ROS TF from %s. Keep disabled if another node already publishes odom -> %s.",
|
||||
odom_topic_.c_str(),
|
||||
base_frame_.c_str());
|
||||
}
|
||||
|
||||
private:
|
||||
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
|
||||
{
|
||||
if (!msg)
|
||||
return;
|
||||
|
||||
geometry_msgs::TransformStamped transform;
|
||||
transform.header = msg->header;
|
||||
transform.child_frame_id = msg->child_frame_id.empty() ? base_frame_ : msg->child_frame_id;
|
||||
transform.transform.translation.x = msg->pose.pose.position.x;
|
||||
transform.transform.translation.y = msg->pose.pose.position.y;
|
||||
transform.transform.translation.z = msg->pose.pose.position.z;
|
||||
transform.transform.rotation = msg->pose.pose.orientation;
|
||||
tf_broadcaster_.sendTransform(transform);
|
||||
}
|
||||
|
||||
std::string base_frame_;
|
||||
std::string odom_topic_;
|
||||
ros::Subscriber odom_sub_;
|
||||
tf2_ros::TransformBroadcaster tf_broadcaster_;
|
||||
};
|
||||
|
||||
class DepthCloudFeeder
|
||||
{
|
||||
public:
|
||||
DepthCloudFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap)
|
||||
: local_costmap_(local_costmap)
|
||||
{
|
||||
private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc"));
|
||||
depth_cloud_sub_ =
|
||||
private_nh.subscribe(depth_cloud_topic_, 1, &DepthCloudFeeder::depthCloudCallback, this);
|
||||
}
|
||||
|
||||
private:
|
||||
void depthCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
|
||||
{
|
||||
if (!msg)
|
||||
return;
|
||||
|
||||
const robot_sensor_msgs::PointCloud2 robot_cloud = toRobotPointCloud2(*msg);
|
||||
feedVoxelLayers(local_costmap_, robot_cloud, depth_cloud_topic_);
|
||||
|
||||
ROS_INFO_THROTTLE(5.0, "Fed depth cloud to robot_costmap_2d: %ux%u frame=%s",
|
||||
msg->width,
|
||||
msg->height,
|
||||
msg->header.frame_id.c_str());
|
||||
}
|
||||
|
||||
robot_costmap_2d::Costmap2DROBOT& local_costmap_;
|
||||
std::string depth_cloud_topic_;
|
||||
ros::Subscriber depth_cloud_sub_;
|
||||
};
|
||||
|
||||
class DepthCameraDataFeeder
|
||||
{
|
||||
public:
|
||||
DepthCameraDataFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap)
|
||||
: local_costmap_(local_costmap)
|
||||
{
|
||||
private_nh.param("depth_image_topic", depth_image_topic_, std::string("/camera/depth/image_raw"));
|
||||
private_nh.param("camera_info_topic", camera_info_topic_, std::string("/camera/depth/camera_info"));
|
||||
private_nh.param("depth_camera_data_topic",
|
||||
depth_camera_data_topic_,
|
||||
std::string("/camera/depth/data"));
|
||||
|
||||
camera_info_sub_ =
|
||||
private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this);
|
||||
depth_image_sub_ =
|
||||
private_nh.subscribe(depth_image_topic_, 1, &DepthCameraDataFeeder::depthImageCallback, this);
|
||||
}
|
||||
|
||||
private:
|
||||
void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
|
||||
{
|
||||
if (!msg)
|
||||
return;
|
||||
|
||||
std::lock_guard<std::mutex> lock(camera_info_mutex_);
|
||||
camera_info_ = depth_image_proc::toRobotCameraInfo(*msg);
|
||||
has_camera_info_ = true;
|
||||
|
||||
ROS_INFO_THROTTLE(5.0, "Cached camera_info for depth clearing: %ux%u frame=%s",
|
||||
msg->width,
|
||||
msg->height,
|
||||
msg->header.frame_id.c_str());
|
||||
}
|
||||
|
||||
void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg)
|
||||
{
|
||||
if (!msg)
|
||||
return;
|
||||
|
||||
robot_sensor_msgs::CameraInfo camera_info;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(camera_info_mutex_);
|
||||
if (!has_camera_info_)
|
||||
{
|
||||
ROS_WARN_THROTTLE(5.0,
|
||||
"Waiting for %s before feeding DepthCameraData to robot_costmap_2d",
|
||||
camera_info_topic_.c_str());
|
||||
return;
|
||||
}
|
||||
camera_info = camera_info_;
|
||||
}
|
||||
|
||||
if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(5.0, "Invalid depth camera intrinsics on %s", camera_info_topic_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
if (msg->encoding != "16UC1" && msg->encoding != "mono16" && msg->encoding != "32FC1")
|
||||
{
|
||||
ROS_ERROR_THROTTLE(
|
||||
5.0,
|
||||
"Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1",
|
||||
msg->encoding.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData());
|
||||
depth_camera_data->depth = depth_image_proc::toRobotImage(*msg);
|
||||
depth_camera_data->camera_info = camera_info;
|
||||
depth_camera_data->header = depth_camera_data->depth.header;
|
||||
if (depth_camera_data->header.frame_id.empty())
|
||||
depth_camera_data->header.frame_id = depth_camera_data->camera_info.header.frame_id;
|
||||
|
||||
robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data;
|
||||
feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_);
|
||||
|
||||
ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s",
|
||||
msg->width,
|
||||
msg->height,
|
||||
msg->encoding.c_str(),
|
||||
depth_camera_data->header.frame_id.c_str(),
|
||||
depth_camera_data_topic_.c_str());
|
||||
}
|
||||
|
||||
robot_costmap_2d::Costmap2DROBOT& local_costmap_;
|
||||
std::string depth_image_topic_;
|
||||
std::string camera_info_topic_;
|
||||
std::string depth_camera_data_topic_;
|
||||
robot_sensor_msgs::CameraInfo camera_info_;
|
||||
bool has_camera_info_{false};
|
||||
std::mutex camera_info_mutex_;
|
||||
ros::Subscriber depth_image_sub_;
|
||||
ros::Subscriber camera_info_sub_;
|
||||
};
|
||||
|
||||
class RightCameraInfoWatchdog
|
||||
{
|
||||
public:
|
||||
explicit RightCameraInfoWatchdog(ros::NodeHandle& private_nh)
|
||||
{
|
||||
std::string camera_right_info_topic;
|
||||
private_nh.param("camera_right_info_topic",
|
||||
camera_right_info_topic,
|
||||
std::string("/camera_right/depth/camera_info"));
|
||||
|
||||
camera_right_info_sub_ =
|
||||
private_nh.subscribe(camera_right_info_topic, 1, &RightCameraInfoWatchdog::cameraRightInfoCallback, this);
|
||||
}
|
||||
|
||||
private:
|
||||
void cameraRightInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
|
||||
{
|
||||
if (msg)
|
||||
ROS_INFO_THROTTLE(5.0, "Right camera info OK: %ux%u frame=%s",
|
||||
msg->width,
|
||||
msg->height,
|
||||
msg->header.frame_id.c_str());
|
||||
}
|
||||
|
||||
ros::Subscriber camera_right_info_sub_;
|
||||
};
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "depth_local_costmap_noetic_test");
|
||||
|
||||
ros::NodeHandle private_nh("~");
|
||||
ros::AsyncSpinner spinner(2);
|
||||
spinner.start();
|
||||
|
||||
std::string base_frame;
|
||||
bool publish_odom_tf = false;
|
||||
private_nh.param("base_frame", base_frame, std::string("base_link"));
|
||||
private_nh.param("publish_odom_tf", publish_odom_tf, false);
|
||||
|
||||
auto tf2_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
|
||||
tf2_ros::TransformListener tf2_listener(*tf2_buffer);
|
||||
tf3::BufferCore tf3_buffer;
|
||||
RosTfToTf3Bridge tf_bridge(tf2_buffer, tf3_buffer);
|
||||
|
||||
std::unique_ptr<OdomTfBridge> odom_tf_bridge;
|
||||
if (publish_odom_tf)
|
||||
odom_tf_bridge = std::make_unique<OdomTfBridge>(private_nh, base_frame);
|
||||
|
||||
RightCameraInfoWatchdog right_camera_info_watchdog(private_nh);
|
||||
|
||||
try
|
||||
{
|
||||
robot_costmap_2d::Costmap2DROBOT local_costmap("local_costmap", tf3_buffer);
|
||||
DepthCloudFeeder depth_cloud_feeder(private_nh, local_costmap);
|
||||
DepthCameraDataFeeder depth_camera_data_feeder(private_nh, local_costmap);
|
||||
local_costmap.start();
|
||||
|
||||
ROS_INFO("T800 robot_costmap_2d local costmap test started.");
|
||||
|
||||
ros::Rate rate(2.0);
|
||||
while (ros::ok())
|
||||
{
|
||||
robot_geometry_msgs::PoseStamped robot_pose;
|
||||
const bool pose_ok = local_costmap.getRobotPose(robot_pose);
|
||||
const robot_costmap_2d::Costmap2D* costmap = local_costmap.getCostmap();
|
||||
|
||||
if (costmap == nullptr)
|
||||
{
|
||||
ROS_WARN_THROTTLE(2.0, "robot_costmap_2d local costmap pointer is null");
|
||||
}
|
||||
else if (!pose_ok)
|
||||
{
|
||||
ROS_WARN_THROTTLE(
|
||||
2.0,
|
||||
"robot_costmap_2d has no robot pose. Check TF: odom -> %s and cloud frame -> odom.",
|
||||
base_frame.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO_THROTTLE(
|
||||
2.0,
|
||||
"robot_costmap_2d OK: size=%ux%u res=%.3f origin=(%.2f, %.2f) robot=(%.2f, %.2f)",
|
||||
costmap->getSizeInCellsX(),
|
||||
costmap->getSizeInCellsY(),
|
||||
costmap->getResolution(),
|
||||
costmap->getOriginX(),
|
||||
costmap->getOriginY(),
|
||||
robot_pose.pose.position.x,
|
||||
robot_pose.pose.position.y);
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
local_costmap.stop();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
ROS_FATAL("depth_local_costmap_noetic_test failed: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user