[Robotflow-cvs] RobotFlow/Behaviors/src Goto.cc,NONE,1.1 Makefile.am,1.12,1.13

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/robotflow/RobotFlow/Behaviors/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv2268

Modified Files:
	Makefile.am 
Added Files:
	Goto.cc 
Log Message:
Add Goto behaviors (debug version)

--- NEW FILE: Goto.cc ---
/* Copyright (C) 2002 Dominic Letourneau (dom...@co...)

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#ifndef _GOTO_CC_
#define _GOTO_CC_

#include "Behavior.h"

using namespace std;

class Goto;

DECLARE_NODE(Goto)
REGISTER_BEHAVIOR(Goto)

/*Node

 * @name Goto
 * @category RobotFlow:Behaviors
 * @description No description available
 *
 * @input_name X
 * @input_type int
 * @input_description absolute target X position (mm)
 *
 * @input_name Y
 * @input_type int
 * @input_description absolute target Y position (mm)
 *
 * @input_name HEADING_POS
 * @input_type int
 * @input_description robot actual heading (degrees)
 *
 * @input_name X_POS
 * @input_type int
 * @input_description robot actual x postion (mm)
 *
 * @input_name Y_POS
 * @input_type int
 * @input_description robot actual y postion (mm)
 *
 * @input_name ACTIVATED
 * @input_type bool
 * @input_description Behavior activation
 *
 * @output_name ROTATION
 * @output_type int
 * @output_description Robot Rotation value (degrees/s)
 *
 * @output_name VELOCITY
 * @output_type int
 * @output_description Robot Velocity (mm/s)
 *
 * @output_name EXPLOITATION
 * @output_description Behavior exploitation
 *
 * @parameter_name MAX_VELOCITY
 * @parameter_description Velocity max (mm/s)
 * @parameter_type int
 * @parameter_value 100
 *
 * @parameter_name MAX_ROTATION
 * @parameter_description Rotation speed max (degrees/s)
 * @parameter_type int
 * @parameter_value 10 
 *
 * @parameter_name HEADING_OFFSET
 * @parameter_description Offset of the heading value with the map orientation(degrees)
 * @parameter_type int
 * @parameter_value 0
 *
 * @parameter_name GOAL_TOLERANCE
 * @parameter_description X and Y tolerance to consider position on goal (mm)
 * @parameter_type int
 * @parameter_value 50
 *
 * @parameter_name HEADING_TOLERANCE
 * @parameter_description  heading tolerance to consider heading to goal (degrees)
 * @parameter_type int
 * @parameter_value 3

END*/

class Goto : public Behavior {

  //inputs
  int xID;
  int yID;
  int headingID;
  int xPosID;
  int yPosID;

  //outputs
  int rotationID;
  int velocityID;

  //parameters
  int m_max_velocity;
  int m_max_rotation;
  int m_heading_offset;
  int m_goal_tol;
  int m_heading_tol;

public:

   Goto(string nodeName, ParameterSet params)
   : Behavior(nodeName, params, "Goto")  {

     //inputs
     xID = addInput("X");
     yID = addInput("Y");
     headingID = addInput("HEADING_POS");
     xPosID = addInput("X_POS");
     yPosID = addInput("Y_POS");

     //outputs
     rotationID = addOutput("ROTATION");
     velocityID = addOutput("VELOCITY");

     m_max_velocity = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
     m_max_rotation = dereference_cast<int> (parameters.get("MAX_ROTATION"));
     m_heading_offset = dereference_cast<int> (parameters.get("HEADING_OFFSET"));
     m_goal_tol = dereference_cast<int> (parameters.get("GOAL_TOLERANCE"));
     m_heading_tol = dereference_cast<int> (parameters.get("HEADING_TOLERANCE"));

   }

   void calculate_behavior(int output_id, int count, Buffer &out) 
   {
     //cerr<<"goto activated"<<endl;
     ObjectRef xValue = getInput(xID,count);
     ObjectRef yValue = getInput(yID,count);
     ObjectRef headingValue = getInput(headingID,count);
     ObjectRef xPosValue = getInput(xPosID,count);
     ObjectRef yPosValue = getInput(yPosID,count);

     if (!xValue->isNil() && !yValue->isNil() && !headingValue->isNil() && !xPosValue->isNil() && !yPosValue->isNil()) 
     {

       int x = dereference_cast<int>(xValue);
       int y = dereference_cast<int>(yValue);
       int heading = dereference_cast<int>(headingValue);
       int xpos = dereference_cast<int>(xPosValue);
       int ypos = dereference_cast<int>(yPosValue);

       if(xpos < x - m_goal_tol || xpos > x + m_goal_tol || ypos < y - m_goal_tol || ypos > y + m_goal_tol)
       {
          double phi = atan((double)((ypos-y)/(xpos-x)));

          double newHeading = heading;
          std::cout << "heading: " << heading << std::endl;

          if(xpos > x)
          {
             if(ypos > y)
             {
                newHeading = M_PI + phi;
             }
             else
             {
                newHeading = M_PI - phi;
             }
          }
          else
          {
             if(ypos > y)
             {
                newHeading = 0 - phi;
             }
             else
             {
                newHeading = 0 + phi;
             }
          }

          newHeading = ((180 * newHeading) / M_PI) + m_heading_offset;

          double diffHeading = heading - newHeading;

          std::cout << "newHeading: " << newHeading << std::endl;
          std::cout << "diffHeading: " << diffHeading << std::endl;

          if(abs(diffHeading) < m_heading_tol)
          {
             (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
             (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(m_max_velocity));
          }
          else
          {
             if(diffHeading > 0 || diffHeading > 180)
             {
                (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(-m_max_rotation));
                (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
             }
             else
             {
                (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(m_max_rotation));
                (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
             }
          }

          std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
          std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
          std::cout << std::endl;
       }
       else
       {
          (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
          (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
          std::cout << "onGoal" << std::endl;
          std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
          std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
          std::cout << std::endl;
       }
     }//object valid
     else 
     {
        (*outputs[rotationID].buffer)[count] = nilObject;
        (*outputs[velocityID].buffer)[count] = nilObject;
     }

   }//calculate_behavior

};
#endif

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Makefile.am,v
retrieving revision 1.12
retrieving revision 1.13
diff -C2 -d -r1.12 -r1.13
*** Makefile.am	6 Aug 2004 13:24:22 -0000	1.12
--- Makefile.am	4 Feb 2005 14:46:38 -0000	1.13
***************
*** 29,33 ****
  	Recommandation.cc \
  	SignTracking.cc \
! 	MultiSignTracking.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)
--- 29,34 ----
  	Recommandation.cc \
  	SignTracking.cc \
! 	MultiSignTracking.cc \
!    Goto.cc

  libBehaviors_la_LDFLAGS = -release $(LT_RELEASE)