[Personalrobots-commit] SF.net SVN: personalrobots:[5066] pkg/trunk

[<tpr...@us...>]

Revision: 5066
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5066&view=rev
Author:   tpratkanis
Date:     2008-10-05 21:05:36 +0000 (Sun, 05 Oct 2008)

Log Message:
-----------
Fix bug 389: not using named path output in the name-value service.

Modified Paths:
--------------
    pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
    pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
    pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv

Added Paths:
-----------
    pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg

Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp	2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp	2008-10-05 21:05:36 UTC (rev 5066)
@@ -295,14 +295,15 @@
 
 /**
  * @brief Iterate over all published joint values we match on
+ * @todo Does not handle joints with multiple axes.
  */
 bool MoveArm::withinBounds(unsigned waypointIndex){
   double sum_joint_diff = 0.0;
   mechanismState.lock();
-  for(unsigned int i=0; i<jointNames_.size(); i++){
+  for(unsigned int i=0; i<plan.path.states[waypointIndex].get_joints_size(); i++){
     double value;
-    if(readJointValue(mechanismState, jointNames_[i], value))
-       sum_joint_diff += fabs(value - plan.path.states[waypointIndex].vals[i]);
+    if(readJointValue(mechanismState, plan.path.states[waypointIndex].names[i], value))
+       sum_joint_diff += fabs(value - plan.path.states[waypointIndex].joints[i].vals[0]);
   }
   mechanismState.unlock();
 
@@ -325,21 +326,26 @@
 
   setCommandParameters(armCommand);
   
-  std::cout << "Dispatching state for waypoint [" << currentWaypoint << "]: " 
-	    << plan.path.states[currentWaypoint].vals[0] << " "
-	    << plan.path.states[currentWaypoint].vals[1] << " "
-	    << plan.path.states[currentWaypoint].vals[2] << " "
-	    << plan.path.states[currentWaypoint].vals[3] << " "
-	    << plan.path.states[currentWaypoint].vals[4] << " "
-	    << plan.path.states[currentWaypoint].vals[5] << " "
-	    << plan.path.states[currentWaypoint].vals[6] << std::endl;
+  std::cout << "Dispatching state for waypoint [" << currentWaypoint << "]: ";
+  
+  for(unsigned int i=0; i<plan.path.states[currentWaypoint].get_joints_size(); i++){
+    std::cout << plan.path.states[currentWaypoint].names[i] << " (";
+    for (unsigned int k=0; k<plan.path.states[currentWaypoint].joints[i].get_vals_size(); k++) {
+      std::cout << plan.path.states[currentWaypoint].joints[i].vals[k] << ",";
+    }
+    std::cout << ") ";
+  }
 
+  std::cout << std::endl;
+
   publish(armCmdTopic, armCommand);
 
   return true;
 }
 
-
+/**
+ * @todo Multi-axis joints.
+ */
 void MoveArm::setCommandParameters(pr2_mechanism_controllers::JointPosCmd& armCommand){
     static const double TOLERANCE(0.25);
 
@@ -349,8 +355,8 @@
     armCommand.set_margins_size(jointNames_.size());
 
     for(unsigned int i = 0; i < jointNames_.size(); i++){
-      armCommand.names[i] = jointNames_[i];
-      armCommand.positions[i] = plan.path.states[currentWaypoint].vals[i];
+      armCommand.names[i] = plan.path.states[currentWaypoint].names[i];
+      armCommand.positions[i] = plan.path.states[currentWaypoint].joints[i].vals[0];
       armCommand.margins[i] = TOLERANCE;
     }
 }

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-10-05 21:05:36 UTC (rev 5066)
@@ -290,8 +290,32 @@
         req.allowed_time = reqn.allowed_time;
         req.threshold = reqn.threshold;
 
+
+
+        //Acutally run the service.
 	bool r = m_requestState.execute(m_models, req, res.path, res.distance);
-        resn.path = res.path;
+
+
+        //Copy the path.
+        resn.path.set_states_size(res.path.get_states_size());
+        
+        for (unsigned int j = 0; j < resn.path.get_states_size(); j++) {
+            nparam = 0;
+            resn.path.states[j].set_names_size(subspaceJointMap.size());
+            resn.path.states[j].set_joints_size(subspaceJointMap.size());
+            for (unsigned int i = 0; i < subspaceJointMap.size(); i++) {
+                resn.path.states[j].names[i] = subspaceJointMap[i]->name;
+                resn.path.states[j].joints[i].set_vals_size(subspaceJointMap[i]->usedParams);
+                for (unsigned int k = 0; k < subspaceJointMap[i]->usedParams; k++) {
+                    resn.path.states[j].joints[i].vals[k] = res.path.states[j].vals[nparam];
+                    nparam++;
+                }
+            }
+        }
+
+        
+
+        //Simply copy the other results.
         resn.distance = res.distance;
         return r;
     }

Added: pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg	                        (rev 0)
+++ pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg	2008-10-05 21:05:36 UTC (rev 5066)
@@ -0,0 +1,3 @@
+# The definition of a kinematic path. Simply a list of states
+
+NamedKinematicState[] states

Modified: pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv	2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv	2008-10-05 21:05:36 UTC (rev 5066)
@@ -45,7 +45,7 @@
 # includes start and goal states) to define the motions for the
 # robot. If more intermediate states are needed, linear interpolation
 # is to be assumed.
-robot_msgs/KinematicPath path
+robot_msgs/NamedKinematicPath path
 
 
 # The threshold that was actually achieved. If the planner did not have enough time,


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.