Merge pull request #16 from trainman419/gazebo2

Update contact manager for Gazebo 2.0
turtlebot · Nov 22, 2014 · ca7201c · ca7201c
2 parents 08e4b32 + 8e0a094
commit ca7201c
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 22 deletions.
diff --git a/create_gazebo_plugins/CATKIN_IGNORE b/create_gazebo_plugins/CATKIN_IGNORE
diff --git a/create_gazebo_plugins/include/turtlebot_plugins/gazebo_ros_create.h b/create_gazebo_plugins/include/turtlebot_plugins/gazebo_ros_create.h
@@ -1,10 +1,12 @@
 #ifndef GAZEBO_ROS_CREATE_H
 #define GAZEBO_ROS_CREATE_H
 
+#include <gazebo/msgs/msgs.hh>
 #include <gazebo/physics/physics.hh>
 #include <gazebo/physics/PhysicsTypes.hh>
 #include <gazebo/sensors/SensorTypes.hh>
 #include <gazebo/transport/TransportTypes.hh>
+#include <gazebo/transport/transport.hh>
 #include <gazebo/common/Time.hh>
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/common/Events.hh>
@@ -19,18 +21,18 @@ namespace gazebo
 {
   class GazeboRosCreate : public ModelPlugin
   {
-    public: 
+    public:
       GazeboRosCreate();
       virtual ~GazeboRosCreate();
-          
+
       virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf );
 
       virtual void UpdateChild();
-  
+
     private:
 
       void UpdateSensors();
-      void OnContact(const std::string &name, const physics::Contact &contact);
+      void OnContact(ConstContactsPtr &contact);
       void OnCmdVel( const geometry_msgs::TwistConstPtr &msg);
 
 
@@ -55,11 +57,11 @@ namespace gazebo
       ros::NodeHandle *rosnode_;
       //ros::Service operating_mode_srv_;
       //ros::Service digital_output_srv_;
-  
+
       ros::Publisher sensor_state_pub_;
       ros::Publisher odom_pub_;
       ros::Publisher joint_state_pub_;
-  
+
       ros::Subscriber cmd_vel_sub_;
 
       physics::WorldPtr my_world_;
@@ -93,7 +95,9 @@ namespace gazebo
       void spin();
       boost::thread *spinner_thread_;
 
-      event::ConnectionPtr contact_event_;
+      //event::ConnectionPtr contact_event_;
+      transport::NodePtr gazebo_node_;
+      transport::SubscriberPtr contact_sub_;
 
       // Pointer to the update event connection
       event::ConnectionPtr updateConnection;

diff --git a/create_gazebo_plugins/src/gazebo_ros_create.cpp b/create_gazebo_plugins/src/gazebo_ros_create.cpp
@@ -11,6 +11,7 @@ using namespace gazebo;
 enum {LEFT= 0, RIGHT=1, FRONT=2, REAR=3};
 
 GazeboRosCreate::GazeboRosCreate()
+  : gazebo_node_(new transport::Node())
 {
   this->spinner_thread_ = new boost::thread( boost::bind( &GazeboRosCreate::spin, this) );
 
@@ -36,7 +37,7 @@ GazeboRosCreate::~GazeboRosCreate()
   delete [] wheel_speed_;
   delete rosnode_;
 }
-    
+
 void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
 {
   this->my_world_ = _parent->GetWorld();
@@ -103,8 +104,11 @@ void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
     }
   }
 
-  base_geom_->SetContactsEnabled(true);
-  contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
+  //base_geom_->SetContactsEnabled(true);
+  //contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
+  physics::ContactManager *mgr = my_world_->GetPhysicsEngine()->GetContactManager();
+  std::string topic = mgr->CreateFilter(base_geom_name_, base_geom_name_);
+  contact_sub_ = gazebo_node_->Subscribe(topic, &GazeboRosCreate::OnContact, this);
 
   wall_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
     sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
@@ -199,22 +203,27 @@ void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
 }
 
 
-void GazeboRosCreate::OnContact(const std::string &name, const physics::Contact &contact)
+void GazeboRosCreate::OnContact(ConstContactsPtr &contacts)
 {
   float y_overlap = 0.16495 * sin( 10 * (M_PI/180.0) );
 
-  for (unsigned int j=0; j < (unsigned int)contact.count; j++)
+  for (int i=0; i < contacts->contact_size(); i++ )
   {
-    // Make sure the contact is on the front bumper
-    if (contact.positions[j].x > 0.012 && contact.positions[j].z < 0.06 && 
-        contact.positions[j].z > 0.01)
+    const msgs::Contact & contact = contacts->contact(i);
+    const int contact_count = contact.position_size();
+    for (unsigned int j=0; j < contact_count; j++)
     {
-      // Right bump sensor
-      if (contact.positions[j].y <= y_overlap)
-        sensor_state_.bumps_wheeldrops |= 0x1; 
-      // Left bump sensor
-      if (contact.positions[j].y >= -y_overlap)
-        sensor_state_.bumps_wheeldrops |= 0x2; 
+      // Make sure the contact is on the front bumper
+      if (contact.position(j).x() > 0.012 && contact.position(j).z() < 0.06 &&
+          contact.position(j).z() > 0.01)
+      {
+        // Right bump sensor
+        if (contact.position(j).y() <= y_overlap)
+          sensor_state_.bumps_wheeldrops |= 0x1;
+        // Left bump sensor
+        if (contact.position(j).y() >= -y_overlap)
+          sensor_state_.bumps_wheeldrops |= 0x2;
+      }
     }
   }
 
@@ -313,7 +322,7 @@ void GazeboRosCreate::UpdateChild()
   odom.twist.twist.angular.y = 0;
   odom.twist.twist.angular.z = odom_vel_[2];
 
-  odom_pub_.publish( odom ); 
+  odom_pub_.publish( odom );
 
   js_.header.stamp.sec = time_now.sec;
   js_.header.stamp.nsec = time_now.nsec;