Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update contact manager for Gazebo 2.0 #16

Merged
merged 1 commit into from
Nov 22, 2014
Merged

Update contact manager for Gazebo 2.0 #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Empty file removed create_gazebo_plugins/CATKIN_IGNORE
View file
Open in desktop
Empty file.
18 changes: 11 additions & 7 deletions create_gazebo_plugins/include/turtlebot_plugins/gazebo_ros_create.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
Expand All @@ -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);


Expand All @@ -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_;
Expand Down Expand Up @@ -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;
Expand Down
39 changes: 24 additions & 15 deletions create_gazebo_plugins/src/gazebo_ros_create.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) );

Expand All @@ -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();
Expand Down Expand Up @@ -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"));
Expand Down Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -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;
Expand Down