mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-07 10:17:45 +08:00
ros2: Ported imu_to_tf and disparity_to_depth nodelets
This commit is contained in:
matlabbe
@@ -25,116 +25,119 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <rtabmap_util/disparity_to_depth.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <stereo_msgs/DisparityImage.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
class DisparityToDepth : public nodelet::Nodelet
|
||||
DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
|
||||
rclcpp::Node("disparity_to_depth", options)
|
||||
{
|
||||
public:
|
||||
DisparityToDepth() {}
|
||||
int qos = 0;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
|
||||
virtual ~DisparityToDepth(){}
|
||||
auto node = rclcpp::Node::make_shared(this->get_name());
|
||||
image_transport::ImageTransport it(node);
|
||||
pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
DisparityToDepth::~DisparityToDepth(){}
|
||||
|
||||
void DisparityToDepth::callback(const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg)
|
||||
{
|
||||
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
image_transport::ImageTransport it(nh);
|
||||
pub32f_ = it.advertise("depth", 1);
|
||||
pub16u_ = it.advertise("depth_raw", 1);
|
||||
sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
|
||||
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1");
|
||||
return;
|
||||
}
|
||||
|
||||
void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
|
||||
bool publish32f = pub32f_.getNumSubscribers();
|
||||
bool publish16u = pub16u_.getNumSubscribers();
|
||||
|
||||
if(publish32f || publish16u)
|
||||
{
|
||||
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
|
||||
// sensor_msgs::image_encodings::TYPE_32FC1
|
||||
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
|
||||
|
||||
cv::Mat depth32f;
|
||||
cv::Mat depth16u;
|
||||
if(publish32f)
|
||||
{
|
||||
NODELET_ERROR("Input type must be disparity=32FC1");
|
||||
return;
|
||||
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
|
||||
}
|
||||
|
||||
bool publish32f = pub32f_.getNumSubscribers();
|
||||
bool publish16u = pub16u_.getNumSubscribers();
|
||||
|
||||
if(publish32f || publish16u)
|
||||
if(publish16u)
|
||||
{
|
||||
// sensor_msgs::image_encodings::TYPE_32FC1
|
||||
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
|
||||
|
||||
cv::Mat depth32f;
|
||||
cv::Mat depth16u;
|
||||
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
|
||||
}
|
||||
float * depth32fPtr=0;
|
||||
unsigned short * depth16uPtr=0;
|
||||
for (int i = 0; i < disparity.rows; ++i)
|
||||
{
|
||||
const float * rowPtr = (const float*)disparity.ptr(i);
|
||||
if(publish32f)
|
||||
{
|
||||
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
|
||||
depth32fPtr = (float*)depth32f.ptr(i);
|
||||
}
|
||||
if(publish16u)
|
||||
{
|
||||
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
|
||||
depth16uPtr = (unsigned short*)depth16u.ptr(i);
|
||||
}
|
||||
for (int i = 0; i < disparity.rows; i++)
|
||||
for (int j = 0; j < disparity.cols; ++j)
|
||||
{
|
||||
for (int j = 0; j < disparity.cols; j++)
|
||||
const float & disparity_value = rowPtr[j];
|
||||
if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
|
||||
{
|
||||
float disparity_value = disparity.at<float>(i,j);
|
||||
if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
|
||||
// baseline * focal / disparity
|
||||
float depth = disparityMsg->t * disparityMsg->f / disparity_value;
|
||||
if(publish32f)
|
||||
{
|
||||
// baseline * focal / disparity
|
||||
float depth = disparityMsg->T * disparityMsg->f / disparity_value;
|
||||
if(publish32f)
|
||||
{
|
||||
depth32f.at<float>(i,j) = depth;
|
||||
}
|
||||
if(publish16u)
|
||||
{
|
||||
depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
|
||||
}
|
||||
depth32fPtr[j] = depth;
|
||||
}
|
||||
if(publish16u)
|
||||
{
|
||||
depth16uPtr[j] = (unsigned short)(depth*1000.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(publish32f)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
|
||||
sensor_msgs::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub32f_.publish(depthMsg);
|
||||
}
|
||||
|
||||
if(publish16u)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
|
||||
sensor_msgs::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub16u_.publish(depthMsg);
|
||||
}
|
||||
}
|
||||
|
||||
if(publish32f)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
|
||||
sensor_msgs::msg::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub32f_.publish(depthMsg);
|
||||
}
|
||||
|
||||
if(publish16u)
|
||||
{
|
||||
// convert to ROS sensor_msg::Image
|
||||
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
|
||||
sensor_msgs::msg::Image depthMsg;
|
||||
cvDepth.toImageMsg(depthMsg);
|
||||
|
||||
//publish the message
|
||||
pub16u_.publish(depthMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
image_transport::Publisher pub32f_;
|
||||
image_transport::Publisher pub16u_;
|
||||
ros::Subscriber sub_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_util::DisparityToDepth, nodelet::Nodelet);
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::DisparityToDepth)
|
||||
|
||||
@@ -25,98 +25,90 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/LinearMath/Matrix3x3.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <rtabmap_util/imu_to_tf.hpp>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
|
||||
class ImuToTF : public nodelet::Nodelet
|
||||
ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) :
|
||||
rclcpp::Node("imu_to_tf", options),
|
||||
fixedFrameId_("odom"),
|
||||
waitForTransformDuration_(0.1)
|
||||
{
|
||||
public:
|
||||
ImuToTF() :
|
||||
fixedFrameId_("odom"),
|
||||
waitForTransformDuration_(0.1)
|
||||
{}
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||
|
||||
virtual ~ImuToTF()
|
||||
{
|
||||
}
|
||||
int qos = 0;
|
||||
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
|
||||
baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
waitForTransformDuration_ = this->declare_parameter("wait_for_transform_duration", waitForTransformDuration_);
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle & nh = getNodeHandle();
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
RCLCPP_INFO(this->get_logger(), "fixed_frame_id: %s", fixedFrameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "base_frame_id: %s", baseFrameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "qos: %d", qos);
|
||||
|
||||
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
|
||||
pnh.param("base_frame_id", baseFrameId_, baseFrameId_);
|
||||
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
|
||||
NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
|
||||
NODELET_INFO("base_frame_id: %s", baseFrameId_.c_str());
|
||||
|
||||
sub_ = nh.subscribe<sensor_msgs::Imu>("imu/data", 1, &ImuToTF::imuCallback, this);
|
||||
}
|
||||
|
||||
void imuCallback(const sensor_msgs::ImuConstPtr & msg)
|
||||
{
|
||||
tf::Quaternion q;
|
||||
tf::quaternionMsgToTF(msg->orientation, q);
|
||||
tf::StampedTransform st;
|
||||
st.setRotation(q);
|
||||
|
||||
st.frame_id_ = fixedFrameId_;
|
||||
st.stamp_ = msg->header.stamp;
|
||||
|
||||
if(!baseFrameId_.empty() &&
|
||||
baseFrameId_.compare(msg->header.frame_id) != 0)
|
||||
{
|
||||
try
|
||||
{
|
||||
std::string errorMsg;
|
||||
if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
|
||||
{
|
||||
NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
|
||||
baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
tf::StampedTransform tmp;
|
||||
tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
|
||||
tf::Transform t = tmp.inverse()*st*tmp;
|
||||
st.setRotation(t.getRotation());
|
||||
st.child_frame_id_ = baseFrameId_;
|
||||
}
|
||||
catch(tf::TransformException & ex)
|
||||
{
|
||||
NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
st.child_frame_id_ = msg->header.frame_id;
|
||||
}
|
||||
st.setOrigin(tf::Vector3(0,0,0));
|
||||
|
||||
pub_.sendTransform(st);
|
||||
}
|
||||
|
||||
private:
|
||||
ros::Subscriber sub_;
|
||||
tf::TransformBroadcaster pub_;
|
||||
std::string fixedFrameId_;
|
||||
std::string baseFrameId_;
|
||||
tf::TransformListener tfListener_;
|
||||
double waitForTransformDuration_;
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_util::ImuToTF, nodelet::Nodelet);
|
||||
sub_ = create_subscription<sensor_msgs::msg::Imu>("imu/data", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ImuToTF::imuCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
ImuToTF::~ImuToTF()
|
||||
{
|
||||
}
|
||||
|
||||
void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
|
||||
{
|
||||
tf2::Quaternion q;
|
||||
tf2::fromMsg(msg->orientation, q);
|
||||
tf2::Transform st;
|
||||
st.setRotation(q);
|
||||
|
||||
std::string childFrameId = msg->header.frame_id;
|
||||
|
||||
if(!baseFrameId_.empty() &&
|
||||
baseFrameId_.compare(msg->header.frame_id) != 0)
|
||||
{
|
||||
try
|
||||
{
|
||||
std::string errorMsg;
|
||||
if(!tfBuffer_->canTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(waitForTransformDuration_), &errorMsg))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
|
||||
baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, rtabmap_conversions::timestampFromROS(msg->header.stamp), errorMsg.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp);
|
||||
tf2::Transform tmp_t;
|
||||
tf2::fromMsg(tmp.transform, tmp_t);
|
||||
tf2::Transform t = tmp_t.inverse()*st*tmp_t;
|
||||
st.setRotation(t.getRotation());
|
||||
childFrameId = baseFrameId_;
|
||||
}
|
||||
catch(tf2::TransformException & ex)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
st.setOrigin(tf2::Vector3(0,0,0));
|
||||
|
||||
geometry_msgs::msg::TransformStamped output;
|
||||
output.header.frame_id = fixedFrameId_;
|
||||
output.header.stamp = msg->header.stamp;
|
||||
output.child_frame_id = childFrameId;
|
||||
output.transform = tf2::toMsg(st);
|
||||
tfBroadcaster_->sendTransform(output);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::ImuToTF)
|
||||
|
||||
Reference in New Issue
Block a user