adding contact information to getRobotCB

CMakeLists.txt

@@ -60,6 +60,7 @@ find_package(graspit)
    SearchSpace.msg
    SearchContact.msg
    Energy.msg
+   Contact.msg
  )
 
 ## Generate services in the 'srv' folder

msg/Contact.msg

@@ -0,0 +1,8 @@
+string body1
+string body2
+
+geometry_msgs/Point position # in body1 frame.
+
+float64 cof # coefficient of friction.
+
+

msg/Robot.msg

@@ -3,3 +3,4 @@ std_msgs/Header header
 geometry_msgs/Pose pose
 sensor_msgs/JointState[] joints
 float64[] dofs
+Contact[] contacts

src/graspit_interface.cpp

@@ -135,9 +135,25 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request,
         pose.orientation.w = t.rotation().w();
         pose.orientation.x = t.rotation().x();
         pose.orientation.y = t.rotation().y();
-        pose.orientation.z = t.rotation().z();
-
-        response.robot.pose = pose;
+	pose.orientation.z = t.rotation().z();
+
+	response.robot.pose = pose;
+
+	// Info for all contacts with this robot:
+	std::list<Contact*> contacts = r->getContacts();
+	for (std::list<Contact*>::iterator it = contacts.begin();
+	     it != contacts.end(); it++) {
+	  graspit_interface::Contact c;
+	  c.body1 = (*it)->getBody1()->getName().toStdString();
+	  c.body2 = (*it)->getBody2()->getName().toStdString();
+
+	  position p = (*it)->getPosition();
+	  c.position.x = p.x() * 0.001; 
+	  c.position.y = p.y() * 0.001; 
+	  c.position.z = p.z() * 0.001; 
+	  c.cof = (*it)->getCof();
+	  response.robot.contacts.push_back(c);
+        }
 
         for (int i=0; i < r->getNumJoints(); i++) {
             sensor_msgs::JointState robot_joint_state = sensor_msgs::JointState();