Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add tf2_ros::MessageFilter for better robot synchronization #39

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add tf2_ros::MessageFilter for better robot synchronization #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 21 additions & 4 deletions include/realtime_urdf_filter/urdf_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,10 @@

#include <GL/freeglut.h>

#include <message_filters/subscriber.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>

namespace realtime_urdf_filter
{

Expand All @@ -62,10 +66,12 @@ class RealtimeURDFFilter
// helper function to get current time
double getTime ();

// callback fucntion that gets ROS image camera info
void cameraInfo_callback(const sensor_msgs::CameraInfo::ConstPtr& current_caminfo);

// callback function that gets ROS images and does everything
void filter_callback
(const sensor_msgs::ImageConstPtr& ros_depth_image,
const sensor_msgs::CameraInfo::ConstPtr& camera_info);
(const sensor_msgs::ImageConstPtr& ros_depth_image);

// does virtual rendering and filtering based on depth buffer and opengl proj. matrix
void filter (
Expand All @@ -84,7 +90,7 @@ class RealtimeURDFFilter
void initFrameBufferObject ();

// compute Projection matrix from CameraInfo message
void getProjectionMatrix (const sensor_msgs::CameraInfo::ConstPtr& current_caminfo, double* glTf);
void getProjectionMatrix (double* glTf);

void render (const double* camera_projection_matrix, ros::Time timestamp = ros::Time());

Expand All @@ -96,9 +102,20 @@ class RealtimeURDFFilter
ros::NodeHandle nh_;
tf::TransformListener tf_;
image_transport::ImageTransport image_transport_;
image_transport::CameraSubscriber depth_sub_;
ros::Subscriber info_sub_;
ros::Publisher realtime_filter_pub_;
image_transport::CameraPublisher depth_pub_;
image_transport::CameraPublisher mask_pub_;

// subscribe camera_info separately because tf2_ros MessageFilter accepts one msg type
sensor_msgs::CameraInfo::ConstPtr camera_info_;

// setup message filter
std::string target_frame_;
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf2_;
message_filters::Subscriber<sensor_msgs::Image> depth_mf_sub_;
tf2_ros::MessageFilter<sensor_msgs::Image> tf2_filter_;

// rendering objects
FramebufferObject *fbo_;
Expand Down
Binary file added src/.urdf_filter.cpp.swp
View file
Open in desktop
Binary file not shown.
47 changes: 28 additions & 19 deletions src/urdf_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,9 @@ RealtimeURDFFilter::RealtimeURDFFilter (ros::NodeHandle &nh, int argc, char **ar
, camera_ty_(0)
, far_plane_ (8)
, near_plane_ (0.1)
, target_frame_("/J1")
, tf2_(buffer_)
, tf2_filter_(depth_mf_sub_, buffer_, target_frame_, 10, 0)
, argc_ (argc), argv_(argv)
{
// get fixed frame name
Expand Down Expand Up @@ -110,12 +113,12 @@ RealtimeURDFFilter::RealtimeURDFFilter (ros::NodeHandle &nh, int argc, char **ar
nh_.param<double> ("filter_replace_value", filter_replace_value_, 0);
ROS_INFO ("using filter replace value %f", filter_replace_value_);

// setup publishers
depth_sub_ = image_transport_.subscribeCamera("input_depth", 10,
&RealtimeURDFFilter::filter_callback, this);
// setup publishers and subscriber
info_sub_ = nh_.subscribe("camera_info", 10, &RealtimeURDFFilter::cameraInfo_callback, this);
depth_mf_sub_.subscribe(nh_, "input_depth", 10);
tf2_filter_.registerCallback(boost::bind(&RealtimeURDFFilter::filter_callback, this,_1));
depth_pub_ = image_transport_.advertiseCamera("output_depth", 10);
mask_pub_ = image_transport_.advertiseCamera("output_mask", 10);
}

RealtimeURDFFilter::~RealtimeURDFFilter ()
{
Expand Down Expand Up @@ -266,13 +269,20 @@ void RealtimeURDFFilter::filter (
}
}

// callback fucntion that gets ROS image camera info
void RealtimeURDFFilter::cameraInfo_callback(const sensor_msgs::CameraInfo::ConstPtr& caminfo)
{
camera_info_ = caminfo;
info_sub_.shutdown();
}

// callback function that gets ROS images and does everything
void RealtimeURDFFilter::filter_callback
(const sensor_msgs::ImageConstPtr& ros_depth_image,
const sensor_msgs::CameraInfo::ConstPtr& camera_info)
void RealtimeURDFFilter::filter_callback(const sensor_msgs::ImageConstPtr& ros_depth_image)
{
// Debugging
ROS_DEBUG_STREAM("Received image with camera info: "<<*camera_info);
if (camera_info_) {
ROS_DEBUG_STREAM("Received image with camera info: "<<camera_info_);
}
// convert to OpenCV cv::Mat
cv_bridge::CvImageConstPtr orig_depth_img;
cv::Mat depth_image;
Expand All @@ -297,7 +307,7 @@ void RealtimeURDFFilter::filter_callback

// Compute the projection matrix from the camera_info
double projection_matrix[16];
getProjectionMatrix (camera_info, projection_matrix);
getProjectionMatrix (projection_matrix);

// Filter the image
this->filter(buffer, projection_matrix, depth_image.cols, depth_image.rows, ros_depth_image->header.stamp);
Expand All @@ -315,7 +325,7 @@ void RealtimeURDFFilter::filter_callback
out_masked_depth.header = ros_depth_image->header;
out_masked_depth.encoding = ros_depth_image->encoding;
out_masked_depth.image = masked_depth_image;
depth_pub_.publish (out_masked_depth.toImageMsg (), camera_info);
depth_pub_.publish (out_masked_depth.toImageMsg (), camera_info_);
}

if (mask_pub_.getNumSubscribers() > 0)
Expand All @@ -325,7 +335,7 @@ void RealtimeURDFFilter::filter_callback
out_mask.header = ros_depth_image->header;
out_mask.encoding = sensor_msgs::image_encodings::MONO8;
out_mask.image = mask_image;
mask_pub_.publish (out_mask.toImageMsg (), camera_info);
mask_pub_.publish (out_mask.toImageMsg (), camera_info_);
}
}

Expand Down Expand Up @@ -456,8 +466,7 @@ void RealtimeURDFFilter::initFrameBufferObject ()
}

// compute Projection matrix from CameraInfo message
void RealtimeURDFFilter::getProjectionMatrix (
const sensor_msgs::CameraInfo::ConstPtr& info, double* glTf)
void RealtimeURDFFilter::getProjectionMatrix (double* glTf)
{
#ifdef USE_OWN_CALIBRATION
float P[12];
Expand All @@ -470,15 +479,15 @@ void RealtimeURDFFilter::getProjectionMatrix (
double cx = P[2];
double cy = P[6];
#else
double fx = info->P[0];
double fy = info->P[5];
double cx = info->P[2];
double cy = info->P[6];
double fx = camera_info_->P[0];
double fy = camera_info_->P[5];
double cx = camera_info_->P[2];
double cy = camera_info_->P[6];

// TODO: check if this does the right thing with respect to registered depth / camera info
// Add the camera's translation relative to the left camera (from P[3]);
camera_tx_ = -1 * (info->P[3] / fx);
camera_ty_ = -1 * (info->P[7] / fy);
camera_tx_ = -1 * (camera_info_->P[3] / fx);
camera_ty_ = -1 * (camera_info_->P[7] / fy);

#endif

Expand Down