88 lines
3.0 KiB
C++
88 lines
3.0 KiB
C++
/*
|
|
* Copyright (C) 2012 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.
|
|
*
|
|
*/
|
|
#include "ContactPlugin.hh"
|
|
|
|
using namespace gazebo;
|
|
GZ_REGISTER_SENSOR_PLUGIN(ContactPlugin)
|
|
|
|
/////////////////////////////////////////////////
|
|
ContactPlugin::ContactPlugin() : SensorPlugin()
|
|
{
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
ContactPlugin::~ContactPlugin()
|
|
{
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void ContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
|
|
{
|
|
// Get the parent sensor.
|
|
this->parentSensor =
|
|
std::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);
|
|
|
|
// Make sure the parent sensor is valid.
|
|
if (!this->parentSensor)
|
|
{
|
|
gzerr << "ContactPlugin requires a ContactSensor.\n";
|
|
return;
|
|
}
|
|
|
|
// Connect to the sensor update event.
|
|
this->updateConnection = this->parentSensor->ConnectUpdated(
|
|
std::bind(&ContactPlugin::OnUpdate, this));
|
|
|
|
// Make sure the parent sensor is active.
|
|
this->parentSensor->SetActive(true);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void ContactPlugin::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";
|
|
}
|
|
}*/
|
|
}
|