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

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

Revision: 5263
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5263&view=rev
Author:   tpratkanis
Date:     2008-10-10 22:49:01 +0000 (Fri, 10 Oct 2008)

Log Message:
-----------
Start fixing arm.1 test.

Modified Paths:
--------------
    pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
    pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
    pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp

Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp	2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp	2008-10-10 22:49:01 UTC (rev 5263)
@@ -80,6 +80,7 @@
 #include <robot_msgs/DisplayKinematicPath.h>
 #include <robot_srvs/NamedKinematicPlanState.h>
 #include <robot_srvs/PlanNames.h>
+#include <rosTF/rosTF.h>
 
 static const unsigned int RIGHT_ARM_JOINTS_BASE_INDEX = 11;
 static const unsigned int LEFT_ARM_JOINTS_BASE_INDEX = 12;
@@ -119,6 +120,7 @@
   mechanism_control::MechanismState mechanismState;
   robot_srvs::NamedKinematicPlanState::response plan;
   unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
+  rosTFClient tf_; /**< Used to do transforms */
 
 protected:
   std::vector<std::string> jointNames_; /*< The collection of joint names of interest. Instantiate in the  derived class.*/
@@ -142,7 +144,7 @@
 MoveArm::MoveArm(const std::string& nodeName, const std::string& stateTopic, const std::string& goalTopic,
 		 const std::string& armPosTopic, const std::string& _armCmdTopic, const std::string& _kinematicModel)
   : HighlevelController<pr2_msgs::MoveArmState, pr2_msgs::MoveArmGoal>(nodeName, stateTopic, goalTopic),
-    armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0){
+    armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0), tf_(*this, true, 10000000000ULL) {
 
 
   // Advertise for messages to command the arm
@@ -213,6 +215,30 @@
   req.times = 1;
 
 
+
+  //Get the pose of the robot:
+  libTF::TFPose2D robotPose, globalPose;
+  robotPose.x = 0;
+  robotPose.y = 0;
+  robotPose.yaw = 0;
+  robotPose.frame = "base";
+  robotPose.time = 0;
+  try{
+    globalPose = this->tf_.transformPose2D("map", robotPose);
+  }
+  catch(libTF::TransformReference::LookupException& ex){
+    std::cout << "No Transform available Error\n";
+  }
+  catch(libTF::TransformReference::ConnectivityException& ex){
+    std::cout << "Connectivity Error\n";
+  }
+  catch(libTF::TransformReference::ExtrapolateException& ex){
+    std::cout << "Extrapolation Error\n";
+  }
+  
+
+
+
   //initializing full value state
   req.start_state.set_names_size(names.get_names_size());
   req.start_state.set_joints_size(names.get_names_size());
@@ -220,8 +246,15 @@
     req.start_state.names[i] = names.names[i];
     //std::cout << req.start_state.names[i] << ": " << names.num_values[i] << std::endl;
     req.start_state.joints[i].set_vals_size(names.num_values[i]);
-    for (int k = 0 ; k < names.num_values[i]; k++) {
-      req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+    if (names.names[i] == "base_joint") {
+      std::cout << "Base: " << i << ", " << globalPose.x << ", " << globalPose.y << ", " << globalPose.yaw << std::endl;
+      req.start_state.joints[i].vals[0] = globalPose.x;
+      req.start_state.joints[i].vals[1] = globalPose.y;
+      req.start_state.joints[i].vals[2] = globalPose.yaw;
+    } else {
+      for (int k = 0 ; k < names.num_values[i]; k++) {
+	req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+      }
     }
   }
 

Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h	2008-10-10 22:49:01 UTC (rev 5263)
@@ -251,7 +251,14 @@
 		for (unsigned int i = 0 ; i < n ; ++i)
 		{
 		    double pos = m_mechanismState.joint_states[i].position;
-		    m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+		    //printf("%d: %s\n", i, m_mechanismState.joint_states[i].name.c_str());
+		    if (m_mechanismState.joint_states[i].name == "base_joint" && m_haveBasePos) {
+        	        m_robotState->setParams(m_basePos, m_mechanismState.joint_states[i].name);
+		    } 
+		    else 
+		    {
+		        m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+		    }
 		}
 	    }	 
 	    if (m_robotStateSimple)

Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp	2008-10-10 22:49:01 UTC (rev 5263)
@@ -188,6 +188,7 @@
   
     void stateUpdate(void)
     {
+      //m_robotState->print();
 	planning_node_util::NodeRobotModel::stateUpdate();
 	if (m_kmodel)
 	    m_kmodel->computeTransforms(m_robotState->getParams());


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