Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10117] pkg/trunk
|
From: <ge...@us...> - 2009-01-24 02:44:29
|
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.
|
