Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18942] pkg/trunk
|
From: <j_b...@us...> - 2009-07-16 01:32:15
|
Revision: 18942
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18942&view=rev
Author: j_bohren
Date: 2009-07-16 01:32:09 +0000 (Thu, 16 Jul 2009)
Log Message:
-----------
Moving writing test and adding trex package for writing executive.
Modified Paths:
--------------
pkg/trunk/stacks/trex/trex_pr2_writing_test/CMakeLists.txt
pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/trex/trex_pr2_writing_core/
pkg/trunk/stacks/trex/trex_pr2_writing_core/CMakeLists.txt
pkg/trunk/stacks/trex/trex_pr2_writing_core/Makefile
pkg/trunk/stacks/trex/trex_pr2_writing_core/cfg/
pkg/trunk/stacks/trex/trex_pr2_writing_core/doc/
pkg/trunk/stacks/trex/trex_pr2_writing_core/doc/writing_master.dia
pkg/trunk/stacks/trex/trex_pr2_writing_core/include/
pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/
pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapter_utilities.h
pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapters.h
pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/master_reactor.h
pkg/trunk/stacks/trex/trex_pr2_writing_core/manifest.xml
pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/
pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/
pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/imports.nddl
pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/master.nddl
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapter_utilities.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapters.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/components.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/main.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/master_reactor.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/src/stub_ros_container.cpp
pkg/trunk/stacks/trex/trex_pr2_writing_core/srv/
pkg/trunk/stacks/trex/trex_pr2_writing_core/srv/ExecuteGoals.srv
pkg/trunk/stacks/trex/trex_pr2_writing_test/
pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch
Removed Paths:
-------------
pkg/trunk/highlevel/writing/writing_test/
pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/CMakeLists.txt 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+#set(ROS_BUILD_TYPE Release)
+rospack(trex_pr2_writing_core)
+
+genmsg()
+gensrv()
+
+rospack_add_boost_directories()
+
+###############################################################################
+# NDDL Include Macro
+###############################################################################
+
+# This generates the NDDL inclue configuration file
+find_ros_package(trex_ros)
+include(${trex_ros_PACKAGE_PATH}/trex_build.cmake)
+create_nddl_config()
+
+###############################################################################
+# TREX Hooks into Local ROS Code
+###############################################################################
+
+set(TREX_PR2_WRITING_CORE_FILES src/adapter_utilities.cpp
+ src/components.cpp
+ src/adapters.cpp
+ src/master_reactor.cpp)
+
+###############################################################################
+# Source library
+###############################################################################
+
+# build extensions into exported library
+create_trex_lib(trex_pr2_writing_core TREX_PR2_WRITING_CORE_FILES)
+
+###############################################################################
+# Executables & Tests
+###############################################################################
+
+create_trex_executables(bin/trexfast bin/trexdebug)
+
+# stub_ros__container implements stub state publishers and actions to allow easy testing of the ROS adapters for the model
+rospack_add_executable(bin/stub_ros_container src/stub_ros_container.cpp)
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/Makefile
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/Makefile (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/Makefile 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/doc/writing_master.dia
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/stacks/trex/trex_pr2_writing_core/doc/writing_master.dia
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapter_utilities.h
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapter_utilities.h (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapter_utilities.h 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,41 @@
+#ifndef H_PR2_ADAPTER_UTILITIES
+#define H_PR2_ADAPTER_UTILITIES
+
+#include "trex_ros/adapter_utilities.h"
+#include <plugs_msgs/PlugStow.h>
+#include <door_msgs/Door.h>
+
+namespace trex_pr2_writing_core {
+
+ /**
+ * @brief A class to scope a set of ros - TREX conversion utilities
+ *
+ * By convention, we write data from TREX to ROS and read data from ROS to TREX. This is reflected
+ * in the use of read and write method qualifiers.
+ */
+ class TempAdapterUtilities : trex_ros::AdapterUtilities {
+ public:
+ /**
+ * @brief Stuff token data into a door message
+ */
+ static void write(const TREX::TokenId& token, door_msgs::Door& msg);
+
+ /**
+ * @brief Read door message into the observation
+ */
+ static void read(TREX::ObservationByValue& obs, const door_msgs::Door& msg);
+
+ /**
+ * @brief Read plug stow message
+ */
+ static void read(TREX::ObservationByValue& obs, const plugs_msgs::PlugStow& msg);
+
+ /**
+ * @brief Stuff token data into a plug stow message
+ */
+ static void write(const TREX::TokenId& token, plugs_msgs::PlugStow& msg);
+ };
+
+}
+
+#endif
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapters.h
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapters.h (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/adapters.h 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,44 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**
+ * @file This file provides shared includes used for stubs and adapters
+ *
+ * @author Conor McGann
+ */
+
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Empty.h>
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/master_reactor.h
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/master_reactor.h (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/include/trex_pr2_writing_core/master_reactor.h 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,23 @@
+#ifndef H_MasterReactor
+#define H_MasterReactor
+
+#include "trex_ros/ros_reactor.h"
+#include "trex_pr2_writing_core/ExecuteGoals.h"
+
+namespace trex_pr2_writing_core {
+
+ class MasterReactor : public trex_ros::ROSReactor {
+ public:
+ MasterReactor(const EUROPA::LabelStr& agentName, const TiXmlElement& configData);
+
+ virtual ~MasterReactor();
+
+ bool executeGoals(ExecuteGoals::Request &req, ExecuteGoals::Response &resp);
+
+ private:
+ ros::NodeHandle node_handle_;
+ ros::ServiceServer service_server_;
+ };
+}
+
+#endif // ifndef H_MasterReactor
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/manifest.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/manifest.xml (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/manifest.xml 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,15 @@
+<package>
+ <description>A ROS node to encapsulate the TeleoReactive Executive for task level planning and control.</description>
+ <author>TREX User</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="trex_pr2" />
+ <depend package="trex_ros" />
+
+ <export>
+ <nddl iflags="-I${prefix}/nddl -I${prefix}"/>
+ <cpp cflags="-I${prefix}/srv/cpp"/>
+ <trex_libs libs="trex_pr2_writing_core"/>
+ </export>
+
+</package>
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/imports.nddl
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/imports.nddl (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/imports.nddl 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,120 @@
+#include "TREX.nddl"
+#include "rcs/exports.nddl"
+#include "rcs/nav/exports.nddl"
+#include "rcs/plugs/exports.nddl"
+#include "nav/exports.nddl"
+#include "plugs/exports.nddl"
+
+/**
+ * Navigation controller will iteratively work towards a goal. It will bite off chunks
+ */
+class Controller extends AgentTimeline {
+ predicate Holds {
+ bool active;
+ }
+
+ NavController(){
+ super(Internal, "Holds");
+ }
+};
+
+/**
+ * The Navigator turns a single navigation goal into something that runs the door controller
+ */
+class Navigator extends Behavior {
+
+ predicate Inactive {}
+
+ /**
+ * When active, we are pursuing a waypoint, although we may be pre-empted
+ */
+ predicate Active{
+ Location w;
+ bool recharging;
+ }
+
+ Navigator(){
+ super(Internal, "Inactive");
+ }
+};
+
+// Internal Timelines
+Navigator navigator = new Navigator();
+NavController nav_controller = new NavController();
+
+// External Timelines
+MoveBehavior driver = new MoveBehavior(External, false);
+MoveBehavior doorman = new MoveBehavior(External, true);
+Recharger recharger = new Recharger(External);
+BaseState base_state = new BaseState(External);
+
+AgentActions actions = new AgentActions();
+
+/**
+ * Planning will generate subgoal so that it is robust to replanning. In real execution we
+ * could rely on subgoaling to a predecessor, and thus not doing it reactively
+ */
+Navigator::Active{
+ contains(NavController.Holds nc);
+ eq(nc.active, true);
+
+ contains(actions.startEvent a);
+ concurrent(a.start, nc.start);
+}
+
+Navigator::Inactive {
+
+}
+
+NavController::Holds {
+
+ if(active == false){
+ contained_by(Navigator.Inactive);
+ }
+
+ met_by(Holds p);
+ defaultOnCommit(active, p.active);
+
+ if(active == true) {
+
+ if(start){
+ // Get the current position: bs(start).
+ any(BaseState.Holds bs);
+ precedes(bs.start, start);
+ temporalDistance(start, [1, +inf], bs.end);
+
+ // Get the target location
+ contained_by(Navigator.Active nav);
+
+ // Are we at the goal?
+ float distance;
+ calcDistance(distance, nav.w.x, nav.w.y, bs.y, bs.y);
+ bool at_goal;
+ testLEQ(at_goal, distance, XY_ERROR);
+
+ // If we are at_goal, terminate
+ if(at_goal == true){
+ eq(duration, 1);
+ meets(Holds s);
+ eq(s.active, false);
+ }
+ else {
+ // Take the next step towards the goal
+ contains(MoveBehavior.Active mb);
+ temporalDistance(mb.end, [-2, 2], end);
+
+ // Determine the next move. if we are doing planar navigation it will simply advance as far
+ // as possible before encountering a door
+ bool thru_doorway;
+ map_get_next_move(mb.x, mb.y, thru_doorway, bs.x, bs.y, nav.w.x, nav.w.y);
+
+ if(thru_doorway == true){
+ eq(mb.object, doorman);
+ }
+ else{
+ eq(mb.object, driver);
+ }
+ }
+ }
+ }
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/master.nddl
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/master.nddl (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/nddl/master/master.nddl 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,317 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the <ORGANIZATION> nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "TREX.nddl"
+#include "world_model/topological_map.nddl"
+#include "rcs/exports.nddl"
+#include "rcs/nav/exports.nddl"
+#include "rcs/plugs/exports.nddl"
+#include "nav/exports.nddl"
+#include "plugs/exports.nddl"
+
+// Priority for goals
+typedef int [0 5] PRIORITY;
+
+// Time Bounds For top level components
+float RECHARGER_DURATION_BOUND = 600.0;
+float DOORMAN_DURATION_BOUND = 600.0;
+float DRIVER_DURATION_BOUND = 600.0;
+
+// Create a behavior timeline class to contain high-level goals
+// Each goal is composed of an Active / Inactive sequence
+class M2Goals extends Behavior {
+
+ // Each goal is composed of an outlet_id, and then a pose in a given frame
+ predicate Active{
+ MAP_KEY outlet_id;
+ string frame_id;
+ float time_stamp;
+ float x;
+ float y;
+ float z;
+ float qx;
+ float qy;
+ float qz;
+ float qw;
+
+ // Constraint must be defined here to allow x and y values to be accessible for planning.
+ map_get_outlet_approach_pose(x, y, z, qx, qy, qz, qw, outlet_id);
+ defaultOnCommit(frame_id, "map");
+ }
+
+ predicate Inactive{}
+
+ M2Goals(){
+ // Call Behavior's constructor
+ // - designating this as an internal timeline
+ super(Internal);
+ }
+};
+
+// Create a timeline class to
+class M2Controller extends AgentTimeline {
+ predicate Holds {
+ bool active;
+ bool recharge_required;
+ bool recharged;
+ bool finished;
+ float x;
+ float y;
+ }
+
+ M2Controller(){
+ // Call AgentTimeline's constructor,
+ // - designating this as an internal timeline
+ // - designating "Holds" as the default predicate
+ super(Internal, "Holds");
+ }
+};
+
+// Instantiate timelines
+
+// Internal Timelines
+M2Goals m2_goals = new M2Goals();
+M2Controller m2_controller = new M2Controller();
+
+// External timelines
+MoveBehavior driver = new MoveBehavior(External, false);
+MoveBehavior doorman = new MoveBehavior(External, true);
+Recharger recharger = new Recharger(External);
+BaseState base_state = new BaseState(External);
+
+M2Goals::Active{
+ // Guard on the built-in variable "start"
+ // This guard will be active when "start" has been closed to a singleton
+ // In other words, it will require nothing until it has been started
+ if(start){
+ // Introduce slave tokens
+ // These temporal constraints allocate new slave tokens:
+ // - m2_controller.Holds "mc"
+ // - base_state.Holds "bs"
+ // "starts" requires that m2_controller.Holds starts at the same time as m2_goals.Active
+ // "starts_during" requires that base_state.Holds starts after m2_goals.Active starts, but before it finishes
+ starts(m2_controller.Holds mc);
+ starts_during(base_state.Holds bs);
+
+ // Impose requirements on mc
+ eq(mc.active, true);
+ // While this goal is active, we should neither be recharged nor finished
+ eq(mc.recharged, false);
+ eq(mc.finished, false);
+
+ // Always synchronize the m2_controler state with basestate
+ eq(mc.x, bs.x);
+ eq(mc.y, bs.y);
+
+ // If the outlet_id for this goal is NO_KEY, then we don't require a recharge
+ if(outlet_id == NO_KEY){
+ eq(mc.recharge_required, false);
+ }
+ else {
+ eq(mc.recharge_required, true);
+ }
+ }
+}
+
+/**
+ * If a goal aborts then we should regenerate it for later
+ */
+M2Goals::Inactive{
+ // Set requirements when the m2_goals.Inactive token is created with status ABORTED
+ if(status == ABORTED){
+ // Allocate slave tokens
+ // An Active token that meets this Inactive token
+ // (the token that has just aborted)
+ met_by(Active p);
+ // An active token that follows this Inactive token
+ // (sometime down the line, not necessary immediately after)
+ before (Active g);
+
+ // Set the token that just failed and the later token to have the same goal
+ eq(g.outlet_id, p.outlet_id);
+ // Set the two slave tokens to have the same pose
+ // This is a hackish thing to account for the lack of struct / user-defined type support in NDDL
+ eq_pose_msg(g.object, p.object);
+ }
+}
+
+/**
+ * The loop for master level control.
+ * It uses the doorman, driver and recharger.
+ */
+M2Controller::Holds{
+ // If the controller is inactive, require that it occurs
+ // at the same time as an Inactive token on the m2_goals timeline
+ if(active == false){
+ equals(m2_goals.Inactive);
+ }
+
+ // Default based on predecessors
+ met_by(Holds p);
+ defaultOnCommit(active, p.active);
+ defaultOnCommit(recharge_required, p.recharge_required);
+ defaultOnCommit(recharged, p.recharged);
+ defaultOnCommit(finished, p.finished);
+ defaultOnCommit(x, p.x);
+ defaultOnCommit(y, p.y);
+
+ // If the controller is active
+ if(active == true){
+ // Allocate a slave token m2_controler.Holds to follow this Holds immediately
+ meets(Holds s);
+ // Allocate a slave token m2_active
+ contained_by(m2_goals.Active m2_active);
+
+ // Test if we are at the goal or not
+ // Declare a local variable distance
+ float distance;
+ // Here x,y are local to m2_controler.Holds, and have been synchronized
+ // with the base_state (the external pose)
+ //
+ // This uses a constraint to close "distance" and calculates the euclidean
+ // distance between the goal m2_active.* and the stored location
+ calcDistance(distance, x, y, m2_active.x, m2_active.y);
+ // Declare a local boolean variable at_goal, and leave it open on [true, false]
+ bool at_goal;
+ // This constrains at_goal to either true or false if distance is less than some proximity
+ testLEQ(at_goal, distance, 0.2);
+
+ // If the controller is active and has finished
+ if(finished == true){
+ // Allocate a slave token m2_inactive to immediately follow this m2_controler.Holds token
+ meets(m2_goals.Inactive m2_inactive);
+ // Require the slave m2_controller.Holds that follows this m2_controller.Holds s.active = false
+ eq(s.active, false);
+ // Require this only lasts for one more tick
+ eq(duration, 1);
+
+ // This sets the following m2_goals slave token to either SUCCESS or ABORTED
+ // Based on whether it achived a required recharge or not
+ if(recharge_required == true){
+ if(recharged == true){
+ eq(m2_inactive.status, SUCCESS);
+ }
+ else {
+ eq(m2_inactive.status, ABORTED);
+ }
+ }
+ else {
+ if(at_goal == true){
+ eq(m2_inactive.status, SUCCESS);
+ }
+ else {
+ eq(m2_inactive.status, ABORTED);
+ }
+ }
+ }
+ // If the controller is active and has not yet finished
+ else {
+ // If we are at the goal pose as estimated above
+ if(at_goal == true){
+ // If a recharge is required
+ if(recharge_required == true){
+ // Allocate a new slave recharger.Active token
+ contains_start(recharger.Active cmd);
+ // Require that this m2_controller.Holds token does not end until recharger.Active has ended
+ concurrent(cmd.end, end);
+ // Require the maximum duration of this delegated action cmd
+ eq(cmd.max_duration, RECHARGER_DURATION_BOUND);
+ // Require the outlet_id of the recharge command is the same as the currently active m2_goals token
+ eq(cmd.outlet_id, m2_active.outlet_id);
+ }
+ // If a recharge is not required
+ else {
+ // Create a new slave token and require that it succeeds like above
+ meets(m2_goals.Inactive m2_inactive);
+ eq(m2_inactive.status, SUCCESS);
+ eq(s.active, false);
+ eq(duration, 1);
+ }
+ }
+ // If we are not at the goal pose as estimated above
+ else{
+ // Take the next step towards the goal. This will either navigate to the goal, or navigate to the
+ // next connector along the way. It would be ideal to be able to specify for the nav stack that
+ // we do not care about orientation, and that we want to get as close as possible to the goal, but the criteria
+ // for success is based on a given error bound. Thus we could get close if possible, but permit a successful termination
+ // when we cannot get any closer but are within that bound. This is desirable since the precise approach is based
+ // on cacluations that deal with position of approach and location of the socket or door and these are encoded internally
+ // in the domains
+ contains_start(MoveBehavior.Active cmd);
+ concurrent(cmd.end, end);
+ eq(cmd.frame_id, m2_active.frame_id);
+
+ // Determine the next map point to go for. It will either be the outlet
+ // or it will be a doorway en-route to the outlet. This function calls a topological planner.
+ bool thru_doorway;
+ map_get_next_move(cmd.x, cmd.y, cmd.z, cmd.qx, cmd.qy, cmd.qz, cmd.qw, thru_doorway, x, y,
+ m2_active.x, m2_active.y, m2_active.z, m2_active.qx, m2_active.qy, m2_active.qz, m2_active.qw);
+
+ map_get_nearest_connector(cmd.start_connector, x, y, m2_active.x, m2_active.y);
+
+ if(thru_doorway == true){
+ // take no more than 10 minutes trying to get thru a doorway
+ eq(cmd.object, doorman);
+ eq(cmd.max_duration, DOORMAN_DURATION_BOUND);
+ }
+ else{
+ eq(cmd.object, driver);
+ eq(cmd.max_duration, DRIVER_DURATION_BOUND);
+ }
+ }
+ }
+ }
+}
+
+/**
+ * The update does not change control states unless it is aborted. Instead, it updates with the new position data
+ */
+MoveBehavior::Inactive{
+ starts(m2_controller.Holds mc);
+ if(status == SUCCESS){
+ eq(mc.x, x);
+ eq(mc.y, y);
+ }
+ else {
+ eq(mc.finished, true);
+ }
+}
+
+Recharger::Inactive{
+ starts(m2_controller.Holds mc);
+ if(status == SUCCESS){
+ eq(mc.finished, true);
+ eq(mc.recharged, true);
+ }
+ else {
+ eq(mc.finished, true);
+ eq(mc.recharged, false);
+ }
+}
+
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapter_utilities.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapter_utilities.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapter_utilities.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,8 @@
+#include "trex_pr2/pr2_adapter_utilities.h"
+#include "trex_ros/adapter_utilities.h"
+#include <tf/transform_listener.h>
+
+namespace trex_pr2_writing_core {
+
+
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapters.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapters.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/adapters.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,48 @@
+/*
+ * TREX Process
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the <ORGANIZATION> nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @file This File contains the declaration and definition of action adapters to bind TREX behavior timeline tokens
+ * to ROS message constructs, and to handle appropriate dispatching and observation synchronization
+ *
+ * @author Conor McGann
+ */
+
+#include <trex_ros/adapter_utilities.h>
+#include "trex_ros/ros_action_adapter.h"
+#include "trex_ros/ros_state_adapter.h"
+
+#include <trex_pr2/pr2_adapter_utilities.h>
+#include <trex_pr2/pr2_adapters.h>
+
+
+namespace trex_pr2_writing_core {
+
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/components.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/components.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/components.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,24 @@
+#include "TeleoReactor.hh"
+#include "Assembly.hh"
+#include "trex_ros/components.h"
+#include <trex_pr2/topological_map.h>
+#include <trex_pr2/door_domain_constraints.h>
+#include <tf/transform_listener.h>
+
+#include "trex_pr2_writing_core/master_reactor.h"
+
+namespace trex_pr2_writing_core{
+ void registerComponents(bool playback, const Assembly& assembly){
+ ConstraintEngineId constraintEngine = ((ConstraintEngine*) assembly.getComponent("ConstraintEngine"))->getId();
+ // See trex_pr2 for custom constraint/solver binfing
+ }
+
+ REGISTER_SCHEMA(trex_pr2_writing_core::registerComponents);
+
+ void registerFactory(bool playback) {
+ // Register special reactors
+ new TREX::TeleoReactor::ConcreteFactory<trex_pr2_writing_core::MasterReactor>("trex_pr2_writing_core_MasterReactor");
+ }
+
+ REGISTER_FACTORY(trex_pr2_writing_core::registerFactory);
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/main.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/main.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/main.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,6 @@
+#include "Assembly.hh"
+#include "trex_ros/executive.h"
+
+int main(int argc, char** argv) {
+ return trexMain(argc, argv);
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/master_reactor.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/master_reactor.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/master_reactor.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,57 @@
+#include "Agent.hh"
+#include "Observer.hh"
+#include "Object.hh"
+#include "Debug.hh"
+#include "Observer.hh"
+#include "Token.hh"
+#include "Domains.hh"
+#include "ConstrainedVariable.hh"
+
+#include "trex_pr2_writing_core/master_reactor.h"
+
+using namespace TREX;
+using namespace EUROPA;
+
+using namespace trex_pr2_writing_core;
+
+/**
+ * ROS Reactors will always log, to support playback. This is achieved by setting parameters in the
+ * base class constructor
+ */
+MasterReactor::MasterReactor(const LabelStr& agentName, const TiXmlElement& configData)
+ : ROSReactor(agentName, configData){
+ service_server_ = node_handle_.advertiseService("execute_goals", &MasterReactor::executeGoals, this);
+}
+
+MasterReactor::~MasterReactor() {}
+
+bool MasterReactor::executeGoals(ExecuteGoals::Request &req, ExecuteGoals::Response &resp){
+ ROS_INFO("ros:execute_goals, Service call made");
+ TREX_INFO("ros:execute_goals", "Service call made");
+
+ ROSReactor::lock();
+
+ // Get the client to work with
+ DbClientId client = getAssembly().getPlanDatabase()->getClient();
+ resp.ok = true;
+
+ for(std::vector<int32_t>::const_iterator it = req.outlet_ids.begin(); it != req.outlet_ids.end(); ++it){
+ // Allocate a token - it should be inactive but not rejectable - cannot deny the truth
+ TokenId token = client->createToken("M2Goals.Active", true);
+ ConstrainedVariableId outlet_id_param = token->getVariable("outlet_id", false);
+ int32_t outlet_id = *it;
+ outlet_id_param->restrictBaseDomain(IntervalIntDomain(outlet_id, outlet_id));
+ if(getAssembly().getConstraintEngine()->propagate()){
+ getGoals().insert(token);
+ }
+ else {
+ ROS_ERROR("Failed to add goal for outlet %d. Skipping.", outlet_id);
+ token->discard();
+ resp.ok = false;
+ }
+ }
+
+ ROSReactor::unlock();
+ return true;
+
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/src/stub_ros_container.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/src/stub_ros_container.cpp (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/src/stub_ros_container.cpp 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,67 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**
+ * @file This file defines a set of actions implemented as stubs so that we can easily
+ * test the ros adapters.
+ *
+ * @author Conor McGann
+ */
+#include <tf/transform_listener.h>
+#include <robot_actions/action.h>
+#include <robot_actions/action_runner.h>
+#include <boost/thread.hpp>
+#include <cstdlib>
+#include <ros/ros.h>
+
+/**
+ * @note This file is temporary. Normally we would not declare and define all these actions together. Just
+ * working with it until we converge on action design model.
+ */
+
+int main(int argc, char** argv){
+ ros::init(argc, argv, "trex_pr2_writing_coreect/action_container");
+ ros::NodeHandle node_handle;
+
+ // Allocate an action runner with an update rate of 10 Hz
+ robot_actions::ActionRunner runner(10.0);
+
+ // Create stubbed actions
+
+ // Run action runner
+ runner.run();
+ ros::spin();
+
+ return 0;
+}
Added: pkg/trunk/stacks/trex/trex_pr2_writing_core/srv/ExecuteGoals.srv
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_core/srv/ExecuteGoals.srv (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_core/srv/ExecuteGoals.srv 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,4 @@
+# For now, a sequence of numbers that are actually outlet ids
+int32[] outlet_ids
+---
+byte ok
\ No newline at end of file
Property changes on: pkg/trunk/stacks/trex/trex_pr2_writing_test
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/writing/writing_test:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/stacks/trex/trex_pr2_writing_test/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/writing/writing_test/CMakeLists.txt 2009-07-15 04:43:05 UTC (rev 18826)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/CMakeLists.txt 2009-07-16 01:32:09 UTC (rev 18942)
@@ -9,7 +9,7 @@
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
-rospack(writing_test)
+rospack(trex_pr2_writing_test)
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Deleted: pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch
===================================================================
--- pkg/trunk/highlevel/writing/writing_test/launch/controllers.launch 2009-07-15 04:43:05 UTC (rev 18826)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch 2009-07-16 01:32:09 UTC (rev 18942)
@@ -1,99 +0,0 @@
-<launch>
-
-<!-- action interface to switch controllers -->
- <node pkg="mechanism_control" type="action_runner" output="screen" />
-
-<!-- Base controller -->
- <param name="base_controller/autostart" type="bool" value="true" />
- <param name="base_controller/odom_publish_rate" value="30.0"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" />
-
-<!-- Laser tilt controller -->
- <param name="laser_tilt_controller/autostart" type="bool" value="false" />
- <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
-
-<!-- Head controller -->
- <param name="head_controller/autostart" type="bool" value="false" />
- <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/head_pan_tilt_controller.xml" />
-
-<!-- Gripper position controller -->
- <param name="r_gripper_position_controller/autostart" type="bool" value="false" />
- <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_gripper_position_controller.xml" />
-
-<!-- Gripper effort controller -->
- <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_gripper_effort_controller.xml" />
-
-
-<!-- Joint Trajectory controller -->
- <group ns="r_arm_joint_trajectory_controller" clear_params="true">
- <param name="velocity_scaling_factor" value="0.50" type="double" />
- <param name="trajectory_wait_timeout" value="10.0" type="double" />
-
- <param name="r_shoulder_pan_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_shoulder_lift_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_upper_arm_roll_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_elbow_flex_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_forearm_roll_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_wrist_flex_joint/goal_reached_threshold" value="0.25" type="double" />
- <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.25" type="double" />
- </group>
- <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
-
-<!-- Cartesian wrench controller -->
- <group ns="r_arm_cartesian_wrench_controller" clear_params="true">
- <param name="autostart" type="bool" value="false" />
- <param name="root_name" type="string" value="torso_lift_link" />
- <param name="tip_name" type="string" value="r_gripper_tool_frame" />
- </group>
-
-<!-- Cartesian twist controller -->
- <group ns="r_arm_cartesian_twist_controller" clear_params="true">
- <param name="root_name" type="string" value="torso_lift_link" />
- <param name="tip_name" type="string" value="r_gripper_tool_frame" />
- <param name="output" type="string" value="r_arm_cartesian_wrench_controller" />
-
- <param name="ff_trans" value="0.0" />
- <param name="ff_rot" value="0.0" />
-
- <param name="fb_trans/p" value="20.0" />
- <param name="fb_trans/i" value="0.5" />
- <param name="fb_trans/d" value="0.0" />
- <param name="fb_trans/i_clamp" value="1.0" />
-
- <param name="fb_rot/p" value="0.5" /> <param name="fb_rot/i" value="0.1" />
- <param name="fb_rot/d" value="0.0" />
- <param name="fb_rot/i_clamp" value="0.2" />
-
- <param name="constraint/active" value="1" />
- <param name="constraint/vel_x" value="0.3" />
- </group>
-
-<!-- Cartesian pose controller -->
- <group ns="r_arm_cartesian_pose_controller" clear_params="true">
- <param name="root_name" type="string" value="torso_lift_link" />
- <param name="tip_name" type="string" value="r_gripper_tool_frame" />
- <param name="output" type="string" value="r_arm_cartesian_twist_controller" />
-
- <param name="p" value="20.0" />
- <param name="i" value="0.1" />
- <param name="d" value="0.0" />
- <param name="i_clamp" value="0.5" />
- </group>
-
-
-<!-- Cartesian trajectory controller -->
- <group ns="r_arm_cartesian_trajectory_controller" clear_params="true">
- <param name="root_name" type="string" value="torso_lift_link" />
- <param name="tip_name" type="string" value="r_gripper_tool_frame" />
- <param name="output" type="string" value="r_arm_cartesian_pose_controller" />
-
- <param name="max_vel_trans" value="0.15" />
- <param name="max_vel_rot" value="0.3" />
- <param name="max_acc_trans" value="0.4" />
- <param name="max_acc_rot" value="0.6" />
- </group>
-
-
-<node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_arm_cartesian_wrench_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_twist_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_pose_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_trajectory_controller.xml" output="screen" />
-
-</launch>
Copied: pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch (from rev 18941, pkg/trunk/highlevel/writing/writing_test/launch/controllers.launch)
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch 2009-07-16 01:32:09 UTC (rev 18942)
@@ -0,0 +1,107 @@
+<launch>
+
+<!-- action interface to switch controllers -->
+ <node pkg="mechanism_control" type="action_runner" output="screen" />
+
+<!-- Base controller -->
+ <param name="base_controller/autostart" type="bool" value="true" />
+ <param name="base_controller/odom_publish_rate" value="30.0"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" />
+
+<!-- Laser tilt controller -->
+ <param name="laser_tilt_controller/autostart" type="bool" value="false" />
+ <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
+
+<!-- Head controller -->
+ <group ns="head_controller" clear_params="true">
+ <param name="pan_link_name" type="string" value="head_pan_link" />
+ <param name="tilt_link_name" type="string" value="head_tilt_link" />
+ <param name="pan_output" type="string" value="head_pan_joint_position_controller" />
+ <param name="tilt_output" type="string" value="head_tilt_joint_position_controller" />
+ </group>
+
+
+ <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/head_pan_joint_position_controller.xml $(find pr2_default_controllers)/head_tilt_joint_position_controller.xml $(find pr2_default_controllers)/head_position_controller.xml" output="screen" />
+
+
+<!-- Gripper position controller -->
+ <param name="r_gripper_position_controller/autostart" type="bool" value="false" />
+ <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_gripper_position_controller.xml" />
+
+<!-- Gripper effort controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_gripper_effort_controller.xml" />
+
+
+<!-- Joint Trajectory controller -->
+ <group ns="r_arm_joint_trajectory_controller" clear_params="true">
+ <param name="velocity_scaling_factor" value="0.50" type="double" />
+ <param name="trajectory_wait_timeout" value="10.0" type="double" />
+
+ <param name="r_shoulder_pan_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_shoulder_lift_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_upper_arm_roll_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_elbow_flex_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_forearm_roll_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_wrist_flex_joint/goal_reached_threshold" value="0.25" type="double" />
+ <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.25" type="double" />
+ </group>
+ <node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
+
+<!-- Cartesian wrench controller -->
+ <group ns="r_arm_cartesian_wrench_controller" clear_params="true">
+ <param name="autostart" type="bool" value="false" />
+ <param name="root_name" type="string" value="torso_lift_link" />
+ <param name="tip_name" type="string" value="r_gripper_tool_frame" />
+ </group>
+
+<!-- Cartesian twist controller -->
+ <group ns="r_arm_cartesian_twist_controller" clear_params="true">
+ <param name="root_name" type="string" value="torso_lift_link" />
+ <param name="tip_name" type="string" value="r_gripper_tool_frame" />
+ <param name="output" type="string" value="r_arm_cartesian_wrench_controller" />
+
+ <param name="ff_trans" value="0.0" />
+ <param name="ff_rot" value="0.0" />
+
+ <param name="fb_trans/p" value="20.0" />
+ <param name="fb_trans/i" value="0.5" />
+ <param name="fb_trans/d" value="0.0" />
+ <param name="fb_trans/i_clamp" value="1.0" />
+
+ <param name="fb_rot/p" value="0.5" /> <param name="fb_rot/i" value="0.1" />
+ <param name="fb_rot/d" value="0.0" />
+ <param name="fb_rot/i_clamp" value="0.2" />
+
+ <param name="constraint/active" value="1" />
+ <param name="constraint/vel_x" value="0.3" />
+ </group>
+
+<!-- Cartesian pose controller -->
+ <group ns="r_arm_cartesian_pose_controller" clear_params="true">
+ <param name="root_name" type="string" value="torso_lift_link" />
+ <param name="tip_name" type="string" value="r_gripper_tool_frame" />
+ <param name="output" type="string" value="r_arm_cartesian_twist_controller" />
+
+ <param name="p" value="20.0" />
+ <param name="i" value="0.1" />
+ <param name="d" value="0.0" />
+ <param name="i_clamp" value="0.5" />
+ </group>
+
+
+<!-- Cartesian trajectory controller -->
+ <group ns="r_arm_cartesian_trajectory_controller" clear_params="true">
+ <param name="root_name" type="string" value="torso_lift_link" />
+ <param name="tip_name" type="string" value="r_gripper_tool_frame" />
+ <param name="output" type="string" value="r_arm_cartesian_pose_controller" />
+
+ <param name="max_vel_trans" value="0.15" />
+ <param name="max_vel_rot" value="0.3" />
+ <param name="max_acc_trans" value="0.4" />
+ <param name="max_acc_rot" value="0.6" />
+ </group>
+
+
+<node pkg="mechanism_control" type="spawner.py" args="--stopped $(find pr2_default_controllers)/r_arm_cartesian_wrench_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_twist_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_pose_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_trajectory_controller.xml" output="screen" />
+
+</launch>
Property changes on: pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/controllers.launch
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml
===================================================================
--- pkg/trunk/highlevel/writing/writing_test/manifest.xml 2009-07-15 04:43:05 UTC (rev 18826)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml 2009-07-16 01:32:09 UTC (rev 18942)
@@ -1,5 +1,5 @@
<package>
- <description brief="writing_test">
+ <description brief="trex_pr2_writing_test">
writing_test
@@ -20,8 +20,8 @@
<depend package="nav_robot_actions"/>
<depend package="point_cloud_assembler"/>
<depend package="2dnav_pr2"/>
-
+ <depend package="trex_pr2_writing_core"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
