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

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

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
====================================...
 
[truncated message content]

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

personalrobots-commit