Program Listing for File gazebo_msgs.hpp
↰ Return to documentation for file (/tmp/ws/src/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp
)
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// 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.
#ifndef GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_
#define GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_
#include <gazebo/msgs/contacts.pb.h>
#include <gazebo_msgs/msg/contact_state.hpp>
#include <gazebo_msgs/msg/contacts_state.hpp>
#include <sstream>
#include "gazebo_ros/conversions/builtin_interfaces.hpp"
#include "gazebo_ros/conversions/generic.hpp"
#include "gazebo_ros/conversions/geometry_msgs.hpp"
namespace gazebo_ros
{
template<class T>
inline
T Convert(const gazebo::msgs::Contacts &)
{
T::ConversionNotImplemented;
}
template<>
inline
gazebo_msgs::msg::ContactsState Convert(const gazebo::msgs::Contacts & in)
{
gazebo_msgs::msg::ContactsState contact_state_msg;
contact_state_msg.header.stamp = Convert<builtin_interfaces::msg::Time>(in.time());
ignition::math::Quaterniond frame_rot;
ignition::math::Vector3d frame_pos;
int contacts_packet_size = in.contact_size();
for (int i = 0; i < contacts_packet_size; ++i) {
// For each collision contact
// Create a ContactState
gazebo_msgs::msg::ContactState state;
const gazebo::msgs::Contact & contact = in.contact(i);
state.collision1_name = contact.collision1();
state.collision2_name = contact.collision2();
std::ostringstream stream;
stream << "Debug: i: (" << i + 1 << "/" << contacts_packet_size <<
") my_geom: [" << state.collision1_name <<
"] other_geom: [" << state.collision2_name <<
"] time: [" << contact.time().sec() << "." << contact.time().nsec() << "]\n";
state.info = stream.str();
// sum up all wrenches for each DOF
geometry_msgs::msg::Wrench total_wrench;
int contact_group_size = contact.position_size();
for (int j = 0; j < contact_group_size; ++j) {
// loop through individual contacts between collision1 and collision2
// Get force, torque and rotate into user specified frame.
// frame_rot is identity if world is used (default for now)
ignition::math::Vector3d force = frame_rot.RotateVectorReverse(
ignition::math::Vector3d(
contact.wrench(j).body_1_wrench().force().x(),
contact.wrench(j).body_1_wrench().force().y(),
contact.wrench(j).body_1_wrench().force().z()));
ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(
ignition::math::Vector3d(
contact.wrench(j).body_1_wrench().torque().x(),
contact.wrench(j).body_1_wrench().torque().y(),
contact.wrench(j).body_1_wrench().torque().z()));
// set wrenches
geometry_msgs::msg::Wrench wrench;
wrench.force = Convert<geometry_msgs::msg::Vector3>(force);
wrench.torque = Convert<geometry_msgs::msg::Vector3>(torque);
state.wrenches.push_back(wrench);
total_wrench.force.x += wrench.force.x;
total_wrench.force.y += wrench.force.y;
total_wrench.force.z += wrench.force.z;
total_wrench.torque.x += wrench.torque.x;
total_wrench.torque.y += wrench.torque.y;
total_wrench.torque.z += wrench.torque.z;
// transform contact positions into relative frame
// set contact positions
ignition::math::Vector3d position = frame_rot.RotateVectorReverse(
ignition::math::Vector3d(
contact.position(j).x(), contact.position(j).y(), contact.position(j).z()) - frame_pos);
auto contact_position = Convert<geometry_msgs::msg::Vector3>(position);
state.contact_positions.push_back(contact_position);
// rotate normal into user specified frame.
// frame_rot is identity if world is used.
ignition::math::Vector3d normal = frame_rot.RotateVectorReverse(
ignition::math::Vector3d(
contact.normal(j).x(), contact.normal(j).y(), contact.normal(j).z()));
// set contact normals
auto contact_normal = Convert<geometry_msgs::msg::Vector3>(normal);
state.contact_normals.push_back(contact_normal);
// set contact depth, interpenetration
state.depths.push_back(contact.depth(j));
}
state.total_wrench = total_wrench;
contact_state_msg.states.push_back(state);
}
return contact_state_msg;
}
} // namespace gazebo_ros
#endif // GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_