Gazebo Collision Model

// Copyright (c) 2019 Steven Macenski
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

This is a gazebo plguin that needs to be added to your URDF / gazebo files in order to make collision detection of the base work.
Please place
  <!-- found name via: gz sdf -p turtlebot3_burger.urdf.xacro > robot.sdf -->
  <gazebo reference="base_footprint">
    <sensor name='collision_sensor' type='contact'>
      <contact>
        <collision>base_footprint_fixed_joint_lump__base_link_collision</collision>
      </contact>
      <plugin name="collision_sensor" filename="libgazebo_collision_sensor.so">
        <rosDebugLevel>na</rosDebugLevel>
        <topicName>/gazebo/base_collision</topicName>
        <updateRate>10.0</updateRate>
      </plugin>
    </sensor>
  </gazebo>

in your gazebo.xacro file. The reference frame should be the global frame of your robot in simulation, which is generally base_footprint. The name of the collision entry can be found via gz sdf -p turtlebot3_burger.urdf.xacro > robot.sdf and reading the robot.sdf file to find the name of the collision object for the base link. We care about base link because that is the recommended place to define the collision object for the base robot and inertia information.

Setting the output topic and update rate are optional.


Msg file: Contact.msg
Header header
string[] objects_hit 



#ifndef _GAZEBO_COLLISION_PLUGIN_H_
#define _GAZEBO_COLLISION_PLUGIN_H_

#include <string>

#include <ros/ros.h>
#include <gazebo/gazebo.hh>
#include <gazebo/util/system.hh>
#include <gazebo/sensors/sensors.hh>
#include <jetson_dqn/Contact.h>
#include <gazebo_plugins/gazebo_ros_utils.h>

namespace gazebo
{

class CollisionPlugin : public SensorPlugin
{

public:
  CollisionPlugin();

  virtual ~CollisionPlugin();

  virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);

private:
  virtual void OnUpdate();
  sensors::ContactSensorPtr parentSensor;
  event::ConnectionPtr updateConnection;
  GazeboRosPtr gazeboRos;
  ros::Publisher contactPub;
};

}

#endif









// Copyright (c) 2019 Steven Macenski
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#include "gazebo_collision_sensor.hpp"

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CollisionPlugin)

/*****************************************************************************/
CollisionPlugin::CollisionPlugin() : SensorPlugin()
/*****************************************************************************/
{
}

/*****************************************************************************/
CollisionPlugin::~CollisionPlugin()
/*****************************************************************************/
{
}

/*****************************************************************************/
void CollisionPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
/*****************************************************************************/
{
  ROS_INFO("CollisionPlugin: Starting contact sensor.");
  std::string topic;
  double rate;
  gazeboRos = GazeboRosPtr(new GazeboRos(_sensor, _sdf, "ContactSensor"));
  gazeboRos->isInitialized();
  gazeboRos->getParameter<std::string> (topic, "topicName", "/gazebo/base_collision" );
  gazeboRos->getParameter<double> (rate, "updateRate", 10.0 );
  contactPub = gazeboRos->node()->advertise<jetson_dqn::Contact>(topic, 5);

  // Get the parent sensor.
  this->parentSensor = std::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);

  // Make sure the parent sensor is valid.
  if (!this->parentSensor)
  {
    gzerr << "CollisionPlugin requires a ContactSensor.\n";
    return;
  }

  this->parentSensor->SetUpdateRate(rate);

  // Connect to the sensor update event.
  this->updateConnection = this->parentSensor->ConnectUpdated(std::bind(&CollisionPlugin::OnUpdate, this));

  // Make sure the parent sensor is active.
  this->parentSensor->SetActive(true);
}

/*****************************************************************************/
void CollisionPlugin::OnUpdate()
/*****************************************************************************/
{
  jetson_dqn::Contact msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = this->parentSensor->ParentName();
  msg.header.frame_id = msg.header.frame_id.substr(msg.header.frame_id.find("::") + 2);
  std::vector<std::string> objs_hit;

  msgs::Contacts contacts;
  contacts = this->parentSensor->Contacts();
  for (unsigned int i = 0; i < contacts.contact_size(); ++i)
  {
    std::string obj_name = contacts.contact(i).collision2();
    while (true)
    {
      if (obj_name.find(std::string("::")) != std::string::npos)
      {
        obj_name = obj_name.substr(obj_name.find("::") + 2);
      }
      else
      {
        break;
      }
    }

    if (obj_name == "ground" || obj_name == "sun")
    {
      continue;
    }

    if (std::find(objs_hit.begin(), objs_hit.end(), obj_name) == objs_hit.end())
    {
      objs_hit.push_back(obj_name);
    }
  }

  if (objs_hit.size() > 0 || contactPub.getNumSubscribers() < 1)
  {
    msg.objects_hit = objs_hit;
    contactPub.publish(msg);    
  }

  return;
}