#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);