From 8e0a094868b8c6ff2eef5c1e17602296390cf859 Mon Sep 17 00:00:00 2001 From: trainman419 Date: Wed, 19 Nov 2014 01:42:38 -0800 Subject: [PATCH] Update contact manager for Gazebo 2.0 --- create_gazebo_plugins/CATKIN_IGNORE | 0 .../turtlebot_plugins/gazebo_ros_create.h | 18 +++++---- .../src/gazebo_ros_create.cpp | 39 ++++++++++++------- 3 files changed, 35 insertions(+), 22 deletions(-) delete mode 100644 create_gazebo_plugins/CATKIN_IGNORE diff --git a/create_gazebo_plugins/CATKIN_IGNORE b/create_gazebo_plugins/CATKIN_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/create_gazebo_plugins/include/turtlebot_plugins/gazebo_ros_create.h b/create_gazebo_plugins/include/turtlebot_plugins/gazebo_ros_create.h index bf4272a..c089b97 100644 --- 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 #include #include #include #include +#include #include #include #include @@ -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 index 0a5fce2..c71a6d0 100644 --- 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::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;