robotskill.cpp

一个简单使用的控制器

#include "RobotSkill.h"
#include <math.h>
#include "util/angles.h"
#include "util/timeutil.h"
#include "util/misc.h"
#include "msg_types.h"
#include "util/robotId.h"
#include "com/ipc_com.h"
#include "logger/src/logger.h"
#include <stdlib.h>

#define DEBUG_PATH 0
#define DEBUG_VICTIM 0
#define DEBUG_RANGES 0
#define DEBUG_REACH_GOAL 0 
#define DEBUG_REACH_GOAL_WITH_OBSTACLES 0
#define DEBUG_FOLLOW_WALL 0
#define DEBUG_FOLLOW_PATH 0
#define DEBUG_FREE_FROM_STALL 0
#define DEBUG_ACTIVE_PERCEPTION 0
#define DEBUG_BEST_ANGLE_FREE_FROM_STALL 0
#define DEBUG_STALL 0 
#define CO2_SENSE_DELAY 10*1000.0 // in ms
#define TURN_AROUND_DELAY 10*1000.0 // in ms
#define RFID_TAG_DEPLOY_TIME 5*1000.0 // in ms

#define MAX_WAIT_TIME_STOP_ROBOT 1*1000.0 // in ms


/**********************************************
 * msgHandlerVictimRFID
 **********************************************/
static void msgHandlerVictimRFID(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_victim_rfid_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg, sizeof(msg));
   IPC_freeByteArray(callData);

   if (DEBUG_VICTIM)
      rlogDebug( "Received VICTIM_RFID from robot (%d)", msg.robot.id);

   ((RobotSkill*)clientData)->updateVictimRFID(false);
}


/**********************************************
 * msgHandlerVictimAck
 **********************************************/
static void msgHandlerVictimAck(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_victim_ack msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(msg));
   IPC_freeByteArray(callData);

   if (DEBUG_VICTIM)
      rlogDebug( "Received VICTIM_ACK from robot (%i)\n", msg.robot.id);

   ((RobotSkill*)clientData)->updateVictimRFID(true);
}


/**********************************************
 * msgHandlerPathMessage
 **********************************************/
   static void msgHandlerPath(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{  if (DEBUG_PATH)
   rlogDebug("msgHandlerPath INVOKED\n");
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_path_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(msg));
   IPC_freeByteArray(callData);

   if (msg.robot.id != getRobotId()) {
      if (DEBUG_PATH)
         rlogDebug("Ignoring PATH_MESSAGE for other robot. own id: %d, msg id: %d\n", getRobotId(), msg.robot.id);
      return;
   }

   if (DEBUG_PATH) {
      rlogDebug( "Received PATH_MESSAGE:  ");
      rlogDebug( "Num Nodes: (%d) ", msg.num);
      rlogDebug( "\n");
   }

   ((RobotSkill*)clientData)->update(msg);
}


RobotSkill::RobotSkill(bool useKalmanPoses, bool simulationMode):
   _lastActivePerception (-1), _angleBeforeTurn(0), _turnAroundSecondHalf(false),
   _activePerceptionDone(true), _initActivePerception(false)
{
   _simulationMode = simulationMode;
   robot = new RobotAction(useKalmanPoses);

   rfidA = new RFIDAction();

   this->path_message.num = 0;
   _tagDeployStartTime.tv_sec=0;
   _tagDeployStartTime.tv_usec=0;
   _victimAck = true;
   _angleHyst=0; 
   gettimeofday(&_activePercetionTimeout, 0);

   ComSubscribeToRobot(RESCUE_PATH_NAME, msgHandlerPath,this);
   ComSubscribeToRobot(RESCUE_VICTIM_RFID_NAME, msgHandlerVictimRFID,this);
   ComSubscribeToRobot(RESCUE_VICTIM_ACK_NAME, msgHandlerVictimAck,this);

   REACHED_DIST=100;

   _rotCmdSpeed=0.0;
   _lastRotCmdSpeed=0.0; 
   _lastInu=0.0;
   _countStall=0;
   _countSmallStall=0;
}


void RobotSkill::update(rescue_path_message &msg) {
   path_message = msg;
   _path.clear();
   Pos p;
   memset(&p, 0, sizeof(p));
   for (int i = 0; i < msg.num; i++) {
      p.x = msg.x[i];
      p.z = msg.y[i];
      p.th = msg.th[i];
      p.timeStamp = msg.robot.ts;
      _path.push_back(p);
   }
}


double RobotSkill::distanceRobotToPoint(double x, double z)
{
   return hypot(x - robot->getPosX(), z - robot->getPosZ());
}    


double RobotSkill::angleRobotToPoint(double x, double z)
{
   double th;
   th = AnglesAtan2(x - robot->getPosX(), z - robot->getPosZ());
   th = AnglesNormAngle(th);
   //rlogDebug("Angle to Point: %f\n",th);
   // rlogDebug("Robot Angle : %f\n",robot->getPosTh());
   th = AnglesSubAngles(th, robot->getPosTh());
   //rlogDebug("Angle to View: %f\n",th);
   th += 180;
   th = AnglesNormAngle(th);
   return(th);
}


void RobotSkill::adjustPathToRobotPos()
{ 
   if (DEBUG_FOLLOW_PATH)
      rlogDebug("APR: Path Length: %d \n", _path.size());
   double minDist = HUGE_VAL;
   double dist;
   int index = -1;
   if (_path.size() <= 1) {
      return;
   }
   for (unsigned int i = 0; i < _path.size() - 1; i++) {
      dist = hypot(robot->getPosX() - _path[i].x, robot->getPosZ() - _path[i].z);
      if (dist <= minDist) {
         minDist = dist;
         index = i;
      }
   }

   for (unsigned int i = index; i < _path.size() - 1; i++) {
      dist = hypot(robot->getPosX() - _path[i].x, robot->getPosZ() - _path[i].z);
      if (dist <= REACHED_DIST) {
         index++;
      }
   }

   if (DEBUG_FOLLOW_PATH)
      rlogDebug("APR: Path index: %d \n", index);
   if (index <= 0) {
      return;
   }
   vector<Pos>::iterator iter;
   iter = _path.begin();
   for(int i = 0; i < index; i++) {
      _path.erase(iter);
      iter++;
   }
}


bool RobotSkill::ActionStop() {
   robot->setVelocity(0, 0);
   _rotCmdSpeed=0;

   return false;
}


bool RobotSkill::ActionFollowPath(bool init, VFHState* vfhState) {
   (void) init;

   if(_path.empty())
      throw "nopath";

   //Pfad an aktuelle Position anpassen
   adjustPathToRobotPos();

   if (DEBUG_FOLLOW_PATH)
      rlogDebug("AFP: Path Length: %d \n", _path.size());

   double go_x;
   double go_y;

   int index=0;
   if (_path.size() > 0) {
      go_x = _path[index].x;
      go_y = _path[index].z;
   } else {
      go_x = robot->getPosX();
      go_y = robot->getPosZ();
   }

   if (DEBUG_FOLLOW_PATH)
      rlogDebug("AFP: Go (dx, dy): (%.2f, %.2f) \n", go_x, go_y);
   Pos goal;
   goal.x = go_x;
   goal.z = go_y;
   /*if(_path.size()==(unsigned int)(index+1)) //turn into direction at the end of the path
   {      // rlogInfo("Setting ending angle %f\n",_path[index].th);
	   goal.th = _path[index].th;
	   ActionReachGoal(true, goal, vfhState,true);

   }
   else
   {  
	   goal.th=0;
	   ActionReachGoal(true, goal, vfhState);
   }*/
    ActionReachGoal(true, goal, vfhState);

   if (DEBUG_FOLLOW_PATH)
	   rlogDebug("AFP: Goal (x, z): (%.2f, %.2f) Robot: (x,z): (%.2f, %.2f) \n", goal.x, goal.z, robot->getPosX(), robot->getPosZ());


   return false;

}

bool RobotSkill::ActionFreeGradient(RobotSkill::Pos goal)
{
	double tVel=0;
	double rVel=0;
	bool done = false; 
	double d = distanceRobotToPoint(goal.x, goal.z);
	double th= angleRobotToPoint(goal.x, goal.z);

   
   // Deciding transvel and rotvel factor
	double transVelFactor, rotVelFactor;
	if (!_simulationMode) {
		//transVelFactor = 700.0;
		transVelFactor = 1000.0;
		rotVelFactor = 500.0;
	}
	else {
		transVelFactor = 500.0;
		rotVelFactor = 100.0;
	}

	if (DEBUG_REACH_GOAL) {
		rlogDebug("REACH_GOAL: Robot x: %.2f z: %.2f th: %.2f \n", robot->getPosX(), robot->getPosZ(), robot->getPosTh());
		rlogDebug("REACH_GOAL: Goal x: %.2f z: %.2f  \n", goal.x, goal.z);
		rlogDebug("REACH_GOAL: Distance to goal: %.2f Angle to goal: %.2f\n", d, th);
	}

        tVel = 0;
	rVel=0;

         
	if (DEBUG_REACH_GOAL) 
		rlogDebug("REACH_GOAL: Not Done.\n"); 
       	

	// Controlling rotVel
	if (!_simulationMode) {
		double w_rot = fabs(StraightUp(fabs(th), 0, 45.0));
		double dir = 0;

		if(th != 0)
			dir = th / fabs(th);

		rVel = dir * w_rot * rotVelFactor;

		if (fabs(rVel) < 40 && fabs(rVel) >= 1)
			rVel=dir*40;
	}
	else {
		double w_rot = fabs(StraightUp(fabs(th), 0, 90.0));
		double dir = 0;

		if(th != 0)
			dir = th / fabs(th);

		rVel = dir * w_rot * rotVelFactor;
		if (fabs(rVel) < 40 && fabs(rVel) >= 1)
			rVel=dir*40;
        }

	// Controlling Tvel
	double w_r = fabs(StraightDown(fabs(th), 0, 20.0));
	double w_trans;
	if (!_simulationMode) 
		w_trans = StraightUp(fabs(d), 0.0, 1000.0);
	else
		w_trans = StraightUp(fabs(d), 0.0, 500.0);

	tVel = w_trans * w_r * transVelFactor; // controlling

	int transVel = (int)  tVel;
        int rotVel = (int) rVel;
	/*if(abs(transVel)<10 && rVel!=0 && fabs(rVel)<10)
                rotVel=(int)(rVel/rVel)*10;
        */
        _rotCmdSpeed=rotVel;
	if ( DEBUG_REACH_GOAL ) 
		rlogDebug("REACH_GOAL: Setting TransVel:  %d, RotVel: %d\n", transVel, rotVel);

	robot->setVelocity(transVel, rotVel);
	return done;
}



bool RobotSkill::ActionReachGoal(bool init, Pos goal, VFHState* vfhState, bool turnAngle)
{
    (void) turnAngle;
	(void) init;
	(void)vfhState;

	double tVel=0;
	double rVel=0;
	bool done = false; 
	double d = distanceRobotToPoint(goal.x, goal.z);
	double th= angleRobotToPoint(goal.x, goal.z);