tf2_ros message filter call back function is never called
Hi, I am simulating husky robot in gazebo environment and trying to publish a marker in order to visualize in rviz. GOAL : Pointstamped obtained from laser scanner (/scan topic ) which is in "baselaser" frame should be converted into "odom" frame and publish message of type "visualizationmsgs::Marker "
I am following tutorials from this page Using stamped data types with tf2. But unfortunately tf2filter call back function is never executed (I understand that call back function will only execute only when transforms are available)
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <visualization_msgs/Marker.h>
namespace husky_highlevel_controller {
class MarkerPublish
{
public:
MarkerPublish(ros::NodeHandle& nodeHandle): node_(nodeHandle), tf2Listener_(tf2Buffer_), target_frame_("odom"), tf2_filter_(point_sub_, tf2Buffer_, target_frame_, 10, 0)
{
ROS_INFO_STREAM("XXXXXXXXXXXXX Entered constructor XXXXXXXXXXXXXXXXX");
// publisher
marker_Visualization_pub = node_.advertise<visualization_msgs::Marker>("/vizMarker", 10);
// published message configuration
marker_msg.ns = "/";
marker_msg.type = visualization_msgs::Marker::SPHERE;
marker_msg.pose.position.z = 0;
marker_msg.color.a = 1.0;
marker_msg.color.g = 1.0;
// subscribing
point_sub_.subscribe(node_, "/closestPoint", 10);
// subscriber callback
tf2_filter_.registerCallback(boost::bind(&MarkerPublish::pointStampedCallback, this, _1));
}
// callback to register with tf2_ros::MessageFilter to be called when transforms are available
void pointStampedCallback(const geometry_msgs::PointStamped::ConstPtr& laserBasePt_ptr)
{
geometry_msgs::PointStamped OdomBasePt;
try {
ROS_DEBUG("Hello %s", "callback - entered");
tf2Buffer_.transform(*laserBasePt_ptr, OdomBasePt, target_frame_);
// marker
marker_msg.header = laserBasePt_ptr->header;
marker_msg.pose.position.x = laserBasePt_ptr->point.x;
marker_msg.pose.position.y = laserBasePt_ptr->point.y;
// publish
marker_Visualization_pub.publish(marker_msg);
ROS_DEBUG("Hello %s", "callback - exist");
}
catch(tf2::TransformException &ex){
ROS_DEBUG("Hello %s", "catch - entered");
ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
}
}
private:
ros::NodeHandle node_;
std::string target_frame_;
tf2_ros::TransformListener tf2Listener_;
tf2_ros::Buffer tf2Buffer_;
message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;
ros::Publisher marker_Visualization_pub;
visualization_msgs::Marker marker_msg;
};
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "vizMarker");
ros::NodeHandle node("~");
husky_highlevel_controller::MarkerPublish marker_publish(node);
ros::spin();
return 0;
};
Asked by BhanuKiran.Chaluvadi on 2017-10-18 12:46:47 UTC
Comments