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

[<is...@us...>]

Revision: 4206
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4206&view=rev
Author:   isucan
Date:     2008-09-11 21:07:50 +0000 (Thu, 11 Sep 2008)

Log Message:
-----------
update display paths so we can reset the state of the robot

Modified Paths:
--------------
    pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
    pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg

Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-09-11 20:56:51 UTC (rev 4205)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp	2008-09-11 21:07:50 UTC (rev 4206)
@@ -241,7 +241,7 @@
 	if (ros::service::call("plan_kinematic_path_position", req, res))
 	{
 	    printPath(res.path, res.distance);
-	    sendDisplay(res.path, req.params.model_id);
+	    sendDisplay(req.start_state, res.path, req.params.model_id);
 	    verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
 	    sendCommand(res.path, req.params.model_id);	    
 	}
@@ -256,7 +256,7 @@
 	if (ros::service::call("plan_kinematic_path_state", req, res))
 	{
 	    printPath(res.path, res.distance);
-	    sendDisplay(res.path, req.params.model_id);
+	    sendDisplay(req.start_state, res.path, req.params.model_id);
 	    verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
 	    sendCommand(res.path, req.params.model_id);
 	}
@@ -298,11 +298,12 @@
 	}
     }
     
-    void sendDisplay(robot_msgs::KinematicPath &path, const std::string &model)
+    void sendDisplay(robot_msgs::KinematicState &start, robot_msgs::KinematicPath &path, const std::string &model)
     {
 	robot_msgs::DisplayKinematicPath dpath;
 	dpath.frame_id = "FRAMEID_MAP";
-	dpath.name = model;
+	dpath.model_name = model;
+	dpath.start_state = start;
 	dpath.path = path;
 	publish("display_kinematic_path", dpath);
     }
@@ -312,7 +313,7 @@
 	// create the service request 
 	//	return;
 	
-	const double margin_fraction = 0.1;
+	//	const double margin_fraction = 0.1;
 	
 	planning_models::KinematicModel::StateParams *state = m_kmodel->newStateParams();
 	pr2_controllers::SetJointTarget::request req;

Modified: pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg	2008-09-11 20:56:51 UTC (rev 4205)
+++ pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg	2008-09-11 21:07:50 UTC (rev 4206)
@@ -1,5 +1,6 @@
 # The definition of a kinematic path that has a name
 # The name identifies the part of the robot the path is for
 string frame_id
-string name
+string model_name
+KinematicState start_state
 KinematicPath path


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