coordInterfaceMissMan.cpp

#include "header/coordInterfaceMissMan.h"

CoordInterfaceMissMan::CoordInterfaceMissMan(ros::NodeHandle nh, std::string robotName){

  this->robotName = robotName;
  this->start = false;
  std::string topicRoot = "/uwsim/" + robotName + "_MissionManager/";

  std::cout << "[" << robotName <<"][COORD_INTERFACE] Start"<<std::endl;

  pubToCoord = nh.advertise<peg_msgs::toCoord>((topicRoot + "toCoord"), 1);

  //pub to let coord that is ready (e.g. to see if both missManager A & B started)
  // ASK towrite in doc  _missionManager" topic are for mission manager topic...
  //withotu missman (only robname) are topic of robotInterface
  readyPub = nh.advertise<std_msgs::Bool>(topicRoot + "ready", 1);

  //sub to know if other robot is ready
  //even if we can read directly the topic of the other robot,
  // for consistency ALL infos received from extern (ie subscribed topic)
  // are given by coordinator.
  // for make each miss manager comunicate with its own robot, there is
  // the robot interface that is another thing
  std::string topicStart = "/uwsim/Coord/StartStopBoth";
  startSub = nh.subscribe(topicStart, 1, &CoordInterfaceMissMan::startSubCallback, this);

  //sub to receive coop vel from coordinator
  std::string topicCoopVel = "/uwsim/Coord/CoopVel";
  coopVelSub = nh.subscribe(topicCoopVel, 1, &CoordInterfaceMissMan::subMMFromCoordCallBack, this);


  //two dimension (it is a matrix)
  toCoordMsg.JJsharp.layout.dim.push_back(std_msgs::MultiArrayDimension());
  toCoordMsg.JJsharp.layout.dim.push_back(std_msgs::MultiArrayDimension());
  toCoordMsg.JJsharp.layout.dim[0].label = "row";
  toCoordMsg.JJsharp.layout.dim[1].label = "column";
  toCoordMsg.JJsharp.layout.dim[0].size = VEHICLE_DOF;
  toCoordMsg.JJsharp.layout.dim[1].size = VEHICLE_DOF;
  toCoordMsg.JJsharp.layout.dim[0].stride = VEHICLE_DOF*VEHICLE_DOF;
  toCoordMsg.JJsharp.layout.dim[1].stride = VEHICLE_DOF;
  toCoordMsg.JJsharp.layout.data_offset = 0;

  if (CHANGE_GOAL){
    updateGoalArrived = false;
    subUpdatedGoal = nh.subscribe("/uwsim/Coord/updatedGoal", 1, &CoordInterfaceMissMan::subUpdatedGoalCallback, this);
  }
}

void CoordInterfaceMissMan::pubIamReadyOrNot(bool ready){
  std_msgs::Bool ready_msg;
  ready_msg.data = ready;
  readyPub.publish(ready_msg);
}

void CoordInterfaceMissMan::startSubCallback(const std_msgs::Bool::ConstPtr& msg){
  start = msg->data;
}

bool CoordInterfaceMissMan::getStartFromCoord(){
  return this->start;
}

int CoordInterfaceMissMan::publishToCoord(Eigen::Matrix<double, VEHICLE_DOF, 1> nonCoopCartVel_eigen,
                                          Eigen::Matrix<double, VEHICLE_DOF, VEHICLE_DOF> admisVelTool_eigen){


  toCoordMsg.xDot.twist.linear.x = nonCoopCartVel_eigen(0);
  toCoordMsg.xDot.twist.linear.y = nonCoopCartVel_eigen(1);
  toCoordMsg.xDot.twist.linear.z = nonCoopCartVel_eigen(2);
  toCoordMsg.xDot.twist.angular.x = nonCoopCartVel_eigen(3);
  toCoordMsg.xDot.twist.angular.y = nonCoopCartVel_eigen(4);
  toCoordMsg.xDot.twist.angular.z = nonCoopCartVel_eigen(5);


  toCoordMsg.JJsharp.data.clear();
  //eigen is column major, multiarray is row major, so we cant simply msg.data = eigen.data
  //and having clear above, we cant directly acces with toCoordMsg.JJsharp.data.at(posRow + j)
  //ASK in this way is it safer? or we can directly overwrite value of previous message (ie without clear?) it would be also faster?
  //faster because every time we dont have to delete memory with clear and reassign with the toCoordMsg.JJsharp.data = temp;
  std::vector<double> temp(VEHICLE_DOF*VEHICLE_DOF, 0);
  int posRow = 0;
  for (int i=0; i<VEHICLE_DOF; i++){
    for (int j=0; j<VEHICLE_DOF; j++){
      posRow = i*VEHICLE_DOF;
      temp[posRow + j] = admisVelTool_eigen(i,j);
    }
  }
  toCoordMsg.JJsharp.data = temp;
  pubToCoord.publish(toCoordMsg);

  return 0;
}


int CoordInterfaceMissMan::getCoopCartVel(
    Eigen::Matrix<double, VEHICLE_DOF, 1> *coopCartVel_eigen){

  int vectSize = tempCoopVel.size();

  if (vectSize == 0){
//    std::cout << "[" << robotName << "][COORD_INTERFACE] WARNING:"
//              << "No xDot_coopVel to read now "
//              << std::endl;
    return 1;
  }

  if (vectSize != VEHICLE_DOF){
    std::cout << "[" << robotName << "][COORD_INTERFACE] ERROR:"
              << "something wrong with dimension of xDot_coopVel received from "
              << "Coordinator" << std::endl;
    return -1;
  }

  for (int i=0; i<vectSize; i++){
    (*coopCartVel_eigen)(i,0) = tempCoopVel.at(i);
  }

  tempCoopVel.clear();

  return 0;
}


void CoordInterfaceMissMan::subMMFromCoordCallBack(const geometry_msgs::TwistStamped& msg){

  tempCoopVel.resize(6); //message is a twist so 6 elements

  tempCoopVel.at(0) = msg.twist.linear.x;
  tempCoopVel.at(1) = msg.twist.linear.y;
  tempCoopVel.at(2) = msg.twist.linear.z;
  tempCoopVel.at(3) = msg.twist.angular.x;
  tempCoopVel.at(4) = msg.twist.angular.y;
  tempCoopVel.at(5) = msg.twist.angular.z;

}


int CoordInterfaceMissMan::getUpdatedGoal(Eigen::Matrix4d *wTgoalTool_eigen){

  if (updateGoalArrived){
    (*wTgoalTool_eigen)(0,3) = updatedPosLin.x;
    (*wTgoalTool_eigen)(1,3) = updatedPosLin.y;
    (*wTgoalTool_eigen)(2,3) = updatedPosLin.z;

    // ang is in a array with COLUMN MAJOR
    (*wTgoalTool_eigen)(0,0) = updatedPosAng.at(0);
    (*wTgoalTool_eigen)(1,0) = updatedPosAng.at(1);
    (*wTgoalTool_eigen)(2,0) = updatedPosAng.at(2);
    (*wTgoalTool_eigen)(0,1) = updatedPosAng.at(3);
    (*wTgoalTool_eigen)(1,1) = updatedPosAng.at(4);
    (*wTgoalTool_eigen)(2,1) = updatedPosAng.at(5);
    (*wTgoalTool_eigen)(0,2) = updatedPosAng.at(6);
    (*wTgoalTool_eigen)(1,2) = updatedPosAng.at(7);
    (*wTgoalTool_eigen)(2,2) = updatedPosAng.at(8);


    updateGoalArrived = false;
    return 0;
  }

  return 1;

}

void CoordInterfaceMissMan::subUpdatedGoalCallback(const peg_msgs::transformMat::ConstPtr &msg){
  updateGoalArrived = true;
  updatedPosLin = msg->linPosition.vector;
  updatedPosAng.resize(9);
  for (int i = 0; i < 9; i++){
    updatedPosAng.at(i) = msg->rotation[i].data;
  }
}



//int CoordInterfaceMissMan::getUpdatedGoal(Eigen::Matrix4d *wTgoalTool_eigen){

//  if (updateGoalArrived){
//    (*wTgoalTool_eigen)(0,3) = updatedPosLin.x;
//    (*wTgoalTool_eigen)(1,3) = updatedPosLin.y;
//    (*wTgoalTool_eigen)(2,3) = updatedPosLin.z;
//    updateGoalArrived = false;
//    return 0;
//  }

//  return 1;

//}

//void CoordInterfaceMissMan::subUpdatedGoalCallback(const geometry_msgs::Vector3Stamped &msg){
//  updateGoalArrived = true;
//  updatedPosLin = msg.vector;
//}