Message filter and tf2 for gazebo bumper plugin
I have a 4 wheel robot in gazebo and I want to get the contact forces to use in my control loop. I am using the gazebo bumper plugin to get the forces, but the problem is that forces are in the wheel's frame. Then I need to synchronize them and transform to the world frame.
I have tried to modify the tf2_ros::MessageFilter Tutorial but something is wrong with the transform function.
How to achieve this transformation?
The code I have tried is pretty the same as the tutorial. I only changed the var type to gazebo_msgs::ContactsState as follows:
#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 "gazebo_msgs/ContactsState.h"
class PoseDrawer
{
public:
PoseDrawer() :
tf2_(buffer_), target_frame_("world"),
tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
{
point_sub_.subscribe(n_, "/pandora/contacts_1", 10);
tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
}
// Callback to register with tf2_ros::MessageFilter to be called when transforms are available
void msgCallback(const gazebo_msgs::ContactsStateConstPtr& point_ptr)
{
gazebo_msgs::ContactsState point_out;
double printVar;
try
{
buffer_.transform(*point_ptr, point_out, target_frame_);
ROS_INFO("Pass");
}
catch (tf2::TransformException &ex)
{
ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
}
}
private:
std::string target_frame_;
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf2_;
ros::NodeHandle n_;
message_filters::Subscriber<gazebo_msgs::ContactsState> point_sub_;
tf2_ros::MessageFilter<gazebo_msgs::ContactsState> tf2_filter_;
};
int main(int argc, char ** argv)
{
ros::init(argc, argv, "pose_drawer"); //Init ROS
PoseDrawer pd; //Construct class
ros::spin(); // Run until interrupted
return 0;
};