[ros-users] Patch for arm_navigation planning_environment co…

Hi All -
I have a patch I'd like to submit (attached) to the planning_environment
package of the arm_navigation stack.

Description below:
-------------------

It seems that no one bothered to copy over the normal and depth information
of contact to the ROS collision message. ODE is already calculating this,
so it is essentially free information being ignored. This seems especially
important since the contact message already has normal and depth fields.
These fields are not currently being set, and all result to 0.

-------------------

Hope that is useful. Perhaps I have missed something and this is being
purposefully ignored. Feel free to respond if this is unclear.

-Joe
From 431a98cf7187412c4209aa575eed8475b3259646 Mon Sep 17 00:00:00 2001
From: Joe Romano <joeromano@gmail.com>
Date: Tue, 21 Feb 2012 10:45:39 -0500
Subject: [PATCH] collision_models: returned depth and normal vector to collision message

---
 .../src/models/collision_models.cpp                |    6 ++++++
 1 files changed, 6 insertions(+), 0 deletions(-)

diff --git a/arm_navigation/planning_environment/src/models/collision_models.cpp b/arm_navigation/planning_environment/src/models/collision_models.cpp
index bce15ce..521e119 100644
--- a/arm_navigation/planning_environment/src/models/collision_models.cpp
+++ b/arm_navigation/planning_environment/src/models/collision_models.cpp
@@ -1408,6 +1408,12 @@ void planning_environment::CollisionModels::getAllCollisionsForState(const plann
     contact_info.position.x = contact.pos.x();
     contact_info.position.y = contact.pos.y();
     contact_info.position.z = contact.pos.z();
+
+    contact_info.normal.x = contact.normal.x();
+    contact_info.normal.y = contact.normal.y();
+    contact_info.normal.z = contact.normal.z();
+    contact_info.depth = contact.depth;
+
     contacts.push_back(contact_info);
   }
   ode_collision_model_->unlock();
-- 
1.7.3.4