Message filter and tf2 for gazebo bumper plugin

asked 2021-02-09 05:34:23 -0500

Hugo gravatar image

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;
};
edit retag flag offensive close merge delete