Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[10117] pkg/trunk (Page 24)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 10117
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10117&view=rev
Author:   gerkey
Date:     2009-01-24 02:44:23 +0000 (Sat, 24 Jan 2009)

Log Message:
-----------
Working on a new version of move_arm, and hacking on a Python executive for
a table-object manipulation experiment.

Modified Paths:
--------------
    pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py
    pkg/trunk/highlevel/executive_python/src/executive.py
    pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
    pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc
    pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
    pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
    pkg/trunk/pr2_msgs/msg/MoveArmState.msg

Modified: pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py
===================================================================

--- pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py	2009-01-24 02:44:23 UTC (rev 10117)
@@ -128,6 +128,9 @@
 from robot_msgs.msg import *
 from highlevel_controllers.msg import *
 from navigation_adapter import *
+#from movearm_adapter import *
+#from tiltlaser_adapter import *
+#from gripper_adapter import *
 
 class Executive:
   def __init__(self, goals, navigator, cycle_time):
@@ -147,10 +150,10 @@
       if self.state == "idle":
         if self.navigator.goalReached() or (not self.navigator.active() and self.current_goal == None) or self.navigator.timeUp():
           self.current_goal = self.goals[random.randint(0, len(self.goals) - 1)]
-          self.navigator.sendGoal(self.current_goal)
+          self.navigator.sendGoal(self.current_goal, "odom")
           print "nav --> nav"
         elif not self.navigator.active() and self.current_goal != None:
-          self.navigator.sendGoal(self.current_goal)
+          self.navigator.sendGoal(self.current_goal, "odom")
           print "nav --> nav"
     else:
       if not self.navigator.legalState():

Modified: pkg/trunk/highlevel/executive_python/src/executive.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/executive.py	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/executive.py	2009-01-24 02:44:23 UTC (rev 10117)
@@ -77,15 +77,15 @@
           print "nav --> recharge"
         elif self.batt_monitor.chargeNeeded():
           chrg_pts = self.chrg_stations[random.randint(0, len(self.chrg_stations) - 1)]
-          self.navigator.sendGoal(chrg_pts)
+          self.navigator.sendGoal(chrg_pts, "map")
           self.state = "nav_charge"
           print "nav --> nav_charge"
         elif self.navigator.goalReached() or (not self.navigator.active() and self.current_goal == None) or self.navigator.timeUp():
           self.current_goal = self.goals[random.randint(0, len(self.goals) - 1)]
-          self.navigator.sendGoal(self.current_goal)
+          self.navigator.sendGoal(self.current_goal, "map")
           print "nav --> nav"
         elif not self.navigator.active() and self.current_goal != None:
-          self.navigator.sendGoal(self.current_goal)
+          self.navigator.sendGoal(self.current_goal, "map")
           print "nav --> nav"
       elif self.state == "recharge":
         """
@@ -94,7 +94,7 @@
         """
         if self.recharger.doneCharging():
           #resume the current goal
-          self.navigator.sendGoal(self.current_goal)
+          self.navigator.sendGoal(self.current_goal, "map")
           self.state = "nav"
           print "recharge --> nav"
       elif self.state == "nav_charge":
@@ -109,7 +109,7 @@
           print "nav_charge --> recharge"
         elif not self.navigator.active() or self.navigator.timeUp():
           chrg_pts = self.chrg_stations[random.randint(0, len(self.chrg_stations) - 1)]
-          self.navigator.sendGoal(chrg_pts)
+          self.navigator.sendGoal(chrg_pts, "map")
           print "nav_charge --> nav_charge"
     else:
       if not self.navigator.legalState():

Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py	2009-01-24 02:44:23 UTC (rev 10117)
@@ -76,10 +76,10 @@
     return [self.state.goal.x, self.state.goal.y, self.state.goal.th]
 
   #Send a new goal to the move base node
-  def sendGoal(self, goal_pts):
+  def sendGoal(self, goal_pts, frame):
     self.start_time = rospy.get_time()
     goal = Planner2DGoal()
-    goal.header.frame_id = "map"
+    goal.header.frame_id = frame
     goal.goal.x = goal_pts[0]
     goal.goal.y = goal_pts[1]
     goal.goal.th = goal_pts[2]

Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ArmControllerAdapter.cc	2009-01-24 02:44:23 UTC (rev 10117)
@@ -18,28 +18,40 @@
   protected:
     
     void fillActiveObservationParameters(ObservationByValue* obs){
+      ROS_FATAL("Disabled, pending update to new MoveArm interface");
+      ROS_BREAK();
+      /*
       for(unsigned int i = 0; i < stateMsg.get_goal_size(); i++){
 	unsigned int ind;
 	if(rosIndex(stateMsg.goal[i].name, ind))
 	   obs->push_back(nddlNames()[ind], new IntervalDomain(stateMsg.goal[i].position));
       }
+      */
     }
 
     void fillInactiveObservationParameters(ObservationByValue* obs){  
+      ROS_FATAL("Disabled, pending update to new MoveArm interface");
+      ROS_BREAK();
+      /*
       for(unsigned int i = 0; i < stateMsg.get_configuration_size(); i++){
 	unsigned int ind;
 	if(rosIndex(stateMsg.configuration[i].name, ind))
 	   obs->push_back(nddlNames()[ind], new IntervalDomain(stateMsg.configuration[i].position));
       }
+      */
     }
 
     void fillRequestParameters(pr2_msgs::MoveArmGoal& goalMsg, const TokenId& goalToken){
+      ROS_FATAL("Disabled, pending update to new MoveArm interface");
+      ROS_BREAK();
+      /*
       goalMsg.set_configuration_size(nddlNames().size());
       for(unsigned int i = 0; i<nddlNames().size(); i++){
 	const IntervalDomain& dom = goalToken->getVariable(nddlNames()[i])->lastDomain();
 	goalMsg.configuration[i].name = rosNames()[i];
 	goalMsg.configuration[i].position = (dom.isSingleton() ? dom.getSingletonValue() : (dom.getUpperBound() + dom.getLowerBound()) / 2);
       }
+      */
     }
   };  
 

Modified: pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt	2009-01-24 02:44:23 UTC (rev 10117)
@@ -3,7 +3,7 @@
 ### If you're trying to debug include directives or so, this here
 ### might help:
 #SET (CMAKE_VERBOSE_MAKEFILE ON)
-set(ROS_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE Debug)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 add_definitions(-Wall)
 rospack(highlevel_controllers)
@@ -15,7 +15,9 @@
 target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d)
 
 # MoveArm
-rospack_add_executable(move_arm src/move_arm.cpp)
+# Eventually move_arm2 will replace move_arm
+#rospack_add_executable(move_arm src/move_arm.cpp)
+rospack_add_executable(move_arm2 src/move_arm2.cpp)
 
 # move_base_sbpl
 rospack_add_executable(move_base_sbpl src/move_base_sbpl.cpp)
@@ -39,4 +41,5 @@
 rospack_add_executable(recharge_controller src/recharge_controller.cpp)
 
 #CLI
-rospack_add_executable(cli src/control_cli.cpp)
+# Disabled, pending update to new message formats
+#rospack_add_executable(cli src/control_cli.cpp)

Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg	2009-01-24 02:44:23 UTC (rev 10117)
@@ -1,7 +1,13 @@
 Header header
-# The desired joint configuratin
-robot_msgs/JointState[] configuration
 
+# Only one of goal_state or goal_constraints is used.  implicit_goal ==
+# 1 means use goal_constraints, 0 means use goal_state
+byte implicit_goal
+# The desired joint state
+robot_msgs/KinematicState goal_state
+# Goal constraints (implicit definition of goal)
+robot_msgs/PoseConstraint[] goal_constraints
+
 # Indicate if the goal is being enabled or disabled
 byte enable
 

Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg	2009-01-24 02:30:21 UTC (rev 10116)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg	2009-01-24 02:44:23 UTC (rev 10117)
@@ -10,6 +10,11 @@
 #Was the planner told to stop?
 byte preempted
 #Current arm configuration
-robot_msgs/JointState[] configuration
-#Goal arm configuration
-robot_msgs/JointState[] goal
+robot_msgs/KinematicState configuration
+# Only one of goal_state or goal_constraints is used.  implicit_goal ==
+# 1 means use goal_constraints, 0 means use goal_state
+byte implicit_goal
+# The desired joint state
+robot_msgs/KinematicState goal_state
+# Goal constraints (implicit definition of goal)
+robot_msgs/PoseConstraint[] goal_constraints


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




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[10117] pkg/trunk (Page 24)

personalrobots-commit