27 #include <gazebo/physics/World.hh> 28 #include <gazebo/physics/HingeJoint.hh> 29 #include <gazebo/physics/Contact.hh> 30 #include <gazebo/sensors/Sensor.hh> 32 #include <sdf/Param.hh> 33 #include <gazebo/common/Exception.hh> 34 #include <gazebo/sensors/SensorTypes.hh> 35 #include <ignition/math/Pose3.hh> 36 #include <ignition/math/Quaternion.hh> 37 #include <ignition/math/Vector3.hh> 47 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper)
70 this->
parentSensor = dynamic_pointer_cast<sensors::ContactSensor>(_parent);
73 ROS_ERROR_NAMED(
"bumper",
"Contact sensor parent is not of type ContactSensor");
78 if (_sdf->HasElement(
"robotNamespace"))
80 _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
85 if (_sdf->HasElement(
"bumperTopicName"))
86 this->bumper_topic_name_ =
87 _sdf->GetElement(
"bumperTopicName")->Get<std::string>();
91 if (!_sdf->HasElement(
"frameName"))
93 ROS_INFO_NAMED(
"bumper",
"bumper plugin missing <frameName>, defaults to world");
97 this->
frame_name_ = _sdf->GetElement(
"frameName")->Get<std::string>();
102 ROS_FATAL_STREAM_NAMED(
"bumper",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 103 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
115 std::string(this->bumper_topic_name_), 1);
138 msgs::Contacts contacts;
143 contacts.time().nsec());
180 ignition::math::Pose3d pose, frame_pose;
181 ignition::math::Quaterniond rot, frame_rot;
182 ignition::math::Vector3d pos, frame_pos;
195 frame_pos = ignition::math::Vector3d(0, 0, 0);
196 frame_rot = ignition::math::Quaterniond(1, 0, 0, 0);
197 frame_pose = ignition::math::Pose3d(frame_pos, frame_rot);
206 unsigned int contactsPacketSize = contacts.contact_size();
207 for (
unsigned int i = 0; i < contactsPacketSize; ++i)
212 gazebo_msgs::ContactState state;
214 gazebo::msgs::Contact contact = contacts.contact(i);
216 state.collision1_name = contact.collision1();
217 state.collision2_name = contact.collision2();
218 std::ostringstream stream;
219 stream <<
"Debug: i:(" << i <<
"/" << contactsPacketSize
220 <<
") my geom:" << state.collision1_name
221 <<
" other geom:" << state.collision2_name
222 <<
" time:" <<
ros::Time(contact.time().sec(), contact.time().nsec())
224 state.info = stream.str();
226 state.wrenches.clear();
227 state.contact_positions.clear();
228 state.contact_normals.clear();
229 state.depths.clear();
232 geometry_msgs::Wrench total_wrench;
233 total_wrench.force.x = 0;
234 total_wrench.force.y = 0;
235 total_wrench.force.z = 0;
236 total_wrench.torque.x = 0;
237 total_wrench.torque.y = 0;
238 total_wrench.torque.z = 0;
240 unsigned int contactGroupSize = contact.position_size();
241 for (
unsigned int j = 0; j < contactGroupSize; ++j)
256 ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
257 contact.wrench(j).body_1_wrench().force().x(),
258 contact.wrench(j).body_1_wrench().force().y(),
259 contact.wrench(j).body_1_wrench().force().z()));
260 ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
261 contact.wrench(j).body_1_wrench().torque().x(),
262 contact.wrench(j).body_1_wrench().torque().y(),
263 contact.wrench(j).body_1_wrench().torque().z()));
266 geometry_msgs::Wrench wrench;
267 wrench.force.x = force.X();
268 wrench.force.y = force.Y();
269 wrench.force.z = force.Z();
270 wrench.torque.x = torque.X();
271 wrench.torque.y = torque.Y();
272 wrench.torque.z = torque.Z();
273 state.wrenches.push_back(wrench);
275 total_wrench.force.x += wrench.force.x;
276 total_wrench.force.y += wrench.force.y;
277 total_wrench.force.z += wrench.force.z;
278 total_wrench.torque.x += wrench.torque.x;
279 total_wrench.torque.y += wrench.torque.y;
280 total_wrench.torque.z += wrench.torque.z;
284 ignition::math::Vector3d position = frame_rot.RotateVectorReverse(
285 ignition::math::Vector3d(contact.position(j).x(),
286 contact.position(j).y(),
287 contact.position(j).z()) - frame_pos);
288 geometry_msgs::Vector3 contact_position;
289 contact_position.x = position.X();
290 contact_position.y = position.Y();
291 contact_position.z = position.Z();
292 state.contact_positions.push_back(contact_position);
296 ignition::math::Vector3d normal = frame_rot.RotateVectorReverse(
297 ignition::math::Vector3d(contact.normal(j).x(),
298 contact.normal(j).y(),
299 contact.normal(j).z()));
301 geometry_msgs::Vector3 contact_normal;
302 contact_normal.x = normal.X();
303 contact_normal.y = normal.Y();
304 contact_normal.z = normal.Z();
305 state.contact_normals.push_back(contact_normal);
308 state.depths.push_back(contact.depth(j));
311 state.total_wrench = total_wrench;
323 static const double timeout = 0.01;
#define ROS_INFO_NAMED(name,...)
std::string bumper_topic_name_
set topic name of broadcast
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL bool isInitialized()
~GazeboRosBumper()
Destructor.
std::string resolve(const std::string &prefix, const std::string &frame_name)
ros::CallbackQueue contact_queue_
void ContactQueueThread()
boost::thread callback_queue_thread_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string robot_namespace_
for setting ROS name space
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
event::ConnectionPtr update_connection_
uint32_t getNumSubscribers() const
void OnContact()
Update the controller.
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
ros::Publisher contact_pub_
ros::NodeHandle * rosnode_
pointer to ros node
sensors::ContactSensorPtr parentSensor
gazebo_msgs::ContactsState contact_state_msg_
broadcast some string for now.