text: /*
   * Copyright (C) 2012-2015 Open Source Robotics Foundation
   *
   * 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.
   * */ /*
   * Desc: Contact plugin
   * Author: Nate Koenig mod by John Hsu
   */
  #include "ModContactPlugin.hh"
  
  using namespace gazebo;
  GZ_REGISTER_SENSOR_PLUGIN(ModContactPlugin)
  
  /////////////////////////////////////////////////
  ModContactPlugin::ModContactPlugin() : SensorPlugin()
  {
  }
  
  /////////////////////////////////////////////////
  ModContactPlugin::~ModContactPlugin()
  {
  }
  
  /////////////////////////////////////////////////
  void ModContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
  {
    // Get the parent sensor.
    this->parentSensor =
      boost::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);
  
    // Make sure the parent sensor is valid.
    if (!this->parentSensor)
    {
      gzerr << "ModContactPlugin requires a ContactSensor.\n";
      return;
    }
  
    // Connect to the sensor update event.
    this->updateConnection = this->parentSensor->ConnectUpdated(
        boost::bind(&ModContactPlugin::OnUpdate, this));
  
    // Make sure the parent sensor is active.
    this->parentSensor->SetActive(true);
  }
  
  /////////////////////////////////////////////////
  void ModContactPlugin::OnUpdate()
  {
    // Uncoment the following lines for debug output
    // Get all the contacts.
    /*msgs::Contacts contacts;
    contacts = this->parentSensor->GetContacts();
    for (int i = 0; i < contacts.contact_size(); ++i)
    {
      std::cout << "Collision between[" << contacts.contact(i).collision1()
                << "] and [" << contacts.contact(i).collision2() << "]\n";
  
      for (int j = 0; j < contacts.contact(i).position_size(); ++j)
      {
        std::cout << j << "  Position:"
                  << contacts.contact(i).position(j).x() << " "
                  << contacts.contact(i).position(j).y() << " "
                  << contacts.contact(i).position(j).z() << "\n";
        std::cout << "   Normal:"
                  << contacts.contact(i).normal(j).x() << " "
                  << contacts.contact(i).normal(j).y() << " "
                  << contacts.contact(i).normal(j).z() << "\n";
        std::cout << "   Depth:" << contacts.contact(i).depth(j) << "\n";
        std::cout << "   Normal force 1: "
                  << contacts.contact(i).normal(j).x() *
                     contacts.contact(i).wrench(j).body_1_wrench().force().x() +
                     contacts.contact(i).normal(j).y() *
                     contacts.contact(i).wrench(j).body_1_wrench().force().y() +
                     contacts.contact(i).normal(j).z() *
                     contacts.contact(i).wrench(j).body_1_wrench().force().z()
                     << "\n";
      }
    }*/
  }
  