coordInterfaceCoord.cpp

#include "header/coordInterfaceCoord.h"

CoordInterfaceCoord::CoordInterfaceCoord(ros::NodeHandle nh, std::string robotNameA,
                                         std::string robotNameB){

  this->robotNameA = robotNameA;
  this->robotNameB = robotNameB;

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

  readyRobA = false;
  readyRobB = false;
  std::string topicReadyA = "/uwsim/" + robotNameA+"_MissionManager"  + "/ready";
  readyRobASub = nh.subscribe(topicReadyA, 1, &CoordInterfaceCoord::readyRobASubCallback, this);
  std::string topicReadyB = "/uwsim/" + robotNameB+"_MissionManager"  + "/ready";
  readyRobBSub = nh.subscribe(topicReadyB, 1, &CoordInterfaceCoord::readyRobBSubCallback, this);


  std::string topicCoordFromMMA = "/uwsim/" + robotNameA +"_MissionManager/toCoord";
  subCoordFromMMA = nh.subscribe(topicCoordFromMMA, 1, &CoordInterfaceCoord::subCoordFromMMACallBack, this);
  std::string topicCoordFromMMB = "/uwsim/" + robotNameB +"_MissionManager/toCoord";
  subCoordFromMMB = nh.subscribe(topicCoordFromMMB, 1, &CoordInterfaceCoord::subCoordFromMMBCallBack, this);

  readxDotA = false;
  readxDotB = false;
  readJJsharpA = false;
  readJJsharpB = false;

  std::string topicStart = "/uwsim/Coord/StartStopBoth";
  startBothPub = nh.advertise<std_msgs::Bool>(topicStart, 1);

  std::string topicCoopVel = "/uwsim/Coord/CoopVel";
  coopVelPub = nh.advertise<geometry_msgs::TwistStamped>(topicCoopVel, 1);

  //change goal things
  if (CHANGE_GOAL){
    subForceTorque = nh.subscribe("/uwsim/g500_A/forceSensorPeg", 1, &CoordInterfaceCoord::subForceTorqueCallback, this);
    vectorForce.resize(3, 0.0); //init as dimension 3 and values all 0.0
    vectorTorque.resize(3, 0.0);
    std::string updateGoalTopic = "/uwsim/Coord/updatedGoal";
    //pubUpdateGoal = nh.advertise<geometry_msgs::Vector3Stamped>(updateGoalTopic, 1);
    pubUpdateGoal = nh.advertise<peg_msgs::transformMat>(updateGoalTopic, 1);
  }
}

bool CoordInterfaceCoord::getReadyBothRob(){
  return (CoordInterfaceCoord::getReadyRobA()
          && CoordInterfaceCoord::getReadyRobB());
}

bool CoordInterfaceCoord::getReadyRobA(){
  return this->readyRobA;
}
bool CoordInterfaceCoord::getReadyRobB(){
  return this->readyRobB;
}

int CoordInterfaceCoord::getMatricesFromRobs(
      Eigen::Matrix<double, VEHICLE_DOF, 1> *nonCoopCartVelA,
      Eigen::Matrix<double, VEHICLE_DOF, 1> *nonCoopCartVelB,
      Eigen::Matrix<double, VEHICLE_DOF, VEHICLE_DOF> *admisVelToolA,
      Eigen::Matrix<double, VEHICLE_DOF, VEHICLE_DOF> *admisVelToolB){

  int return1 = 0;
  int return2 = 0;
  int return3 = 0;
  int return4 = 0;

  if (!readxDotA){
    return1 = CoordInterfaceCoord::getNonCoopCartVel(nonCoopCartVelA, robotNameA);
  }

  if (!readxDotB){
    return2 = CoordInterfaceCoord::getNonCoopCartVel(nonCoopCartVelB, robotNameB);

  }
  if (!readJJsharpA){
    return3 = CoordInterfaceCoord::getJJsharp(admisVelToolA, robotNameA);
  }
  if (!readJJsharpB){
    return4 = CoordInterfaceCoord::getJJsharp(admisVelToolB, robotNameB);
  }

  if (return1 == -1 || return2 ==-1 || return3 ==-1 || return4 ==-1){
    return -1;
  }
  if (return1 == -2 || return2 ==-2 || return3 ==-2 || return4 ==-2){
    return -2;
  }

  int counter = return1+return2+return3+return4;

  //if so, all get returned 0 (message arrived) or someone arent called because called
  //previously and have the flag read to true.
  if (counter == 0){ //reset read flags
    readxDotA = false;
    readxDotB = false;
    readJJsharpA = false;
    readJJsharpB = false;
  }

  return counter;

}

void CoordInterfaceCoord::publishStartBoth(bool start){

  std_msgs::Bool start_msg;
  start_msg.data = start;
  startBothPub.publish(start_msg);

}

void CoordInterfaceCoord::publishCoopVel(Eigen::Matrix<double, VEHICLE_DOF, 1> coopVel){

  geometry_msgs::TwistStamped msg;
  msg.twist.linear.x = coopVel(0);
  msg.twist.linear.y = coopVel(1);
  msg.twist.linear.z = coopVel(2);
  msg.twist.angular.x = coopVel(3);
  msg.twist.angular.y = coopVel(4);
  msg.twist.angular.z = coopVel(5);

  coopVelPub.publish(msg);

}


int CoordInterfaceCoord::getNonCoopCartVel(
    Eigen::Matrix<double, VEHICLE_DOF, 1> *nonCoopCartVel_eigen,
    std::string robotName){

  int vectSize;
  std::vector<double> *tempXdot;
  bool* read;
  if (robotName.compare(robotNameA) == 0){
    vectSize = tempXdotA.size();
    tempXdot = &tempXdotA;
    read = &readxDotA;

  } else if (robotName.compare(robotNameB) == 0){
    vectSize = tempXdotB.size();
    tempXdot = &tempXdotB;
    read = &readxDotB;
  } else {
    return -2;
  }

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

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

  for (int i=0; i<vectSize; i++){
    (*nonCoopCartVel_eigen)(i,0) = tempXdot->at(i);
  }

  tempXdot->clear();
  *read = true;

  return 0;
}

int CoordInterfaceCoord::getJJsharp(
    Eigen::Matrix<double, VEHICLE_DOF, VEHICLE_DOF> *admisVelTool_eigen,
    std::string robotName){


  int nSize;
  std::vector<double> *tempJJsharp;
  bool* read;
  if (robotName.compare(robotNameA) == 0){
    nSize = tempJJsharpA.size();
    tempJJsharp = &tempJJsharpA;
    read = &readJJsharpA;
  } else if (robotName.compare(robotNameB) == 0){
    nSize = tempJJsharpB.size();
    tempJJsharp = &tempJJsharpB;
    read = &readJJsharpB;
  } else {
    return -2;
  }


  if (nSize == 0){
//    std::cout << "[COORDINATOR][COORD_INTERFACE] WARNING:"
//              << "No JJ# to read now "
//              << std::endl;
    return 1;
  }

  if (nSize != VEHICLE_DOF*VEHICLE_DOF){
//    std::cout << "[COORDINATOR][COORD_INTERFACE] ERROR:"
//              << "something wrong with dimension of JJ# received from "
//              << robotName << std::endl;
    return -1;
  }

  //Rembering that eigen is column major and the msg is store row major
  int posRow = 0;
  for (int i=0; i<VEHICLE_DOF; i++){
    for (int j=0; j<VEHICLE_DOF; j++){
      posRow = i*VEHICLE_DOF;
      (*admisVelTool_eigen)(i,j) = tempJJsharp->at(posRow + j);
    }
  }

  tempJJsharp->clear();
  *read = true;
  return 0;
}


void CoordInterfaceCoord::readyRobASubCallback(const std_msgs::Bool::ConstPtr& start){
  readyRobA = start->data;
}
void CoordInterfaceCoord::readyRobBSubCallback(const std_msgs::Bool::ConstPtr& start){
  readyRobB = start->data;
}


void CoordInterfaceCoord::subCoordFromMMACallBack(const peg_msgs::toCoord::ConstPtr& msg){

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

  tempXdotA.at(0) = msg->xDot.twist.linear.x;
  tempXdotA.at(1) = msg->xDot.twist.linear.y;
  tempXdotA.at(2) = msg->xDot.twist.linear.z;
  tempXdotA.at(3) = msg->xDot.twist.angular.x;
  tempXdotA.at(4) = msg->xDot.twist.angular.y;
  tempXdotA.at(5) = msg->xDot.twist.angular.z;


  //dim[0].stride is the total number of element (row*col)
  tempJJsharpA.resize(msg->JJsharp.layout.dim[0].stride);
  tempJJsharpA = msg->JJsharp.data;

}

void CoordInterfaceCoord::subCoordFromMMBCallBack(const peg_msgs::toCoord::ConstPtr& msg){

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

  tempXdotB.at(0) = msg->xDot.twist.linear.x;
  tempXdotB.at(1) = msg->xDot.twist.linear.y;
  tempXdotB.at(2) = msg->xDot.twist.linear.z;
  tempXdotB.at(3) = msg->xDot.twist.angular.x;
  tempXdotB.at(4) = msg->xDot.twist.angular.y;
  tempXdotB.at(5) = msg->xDot.twist.angular.z;


  //dim[0].stride is the total number of element (row*col)
  tempJJsharpB.resize(msg->JJsharp.layout.dim[0].stride);
  tempJJsharpB = msg->JJsharp.data;

}

int CoordInterfaceCoord::getForceTorque(Eigen::Vector3d *force, Eigen::Vector3d *torque){

  *force = CONV::vector_std2Eigen(vectorForce);
  *torque = CONV::vector_std2Eigen(vectorTorque);

  return 0;
}

void CoordInterfaceCoord::subForceTorqueCallback(const geometry_msgs::WrenchStamped& msg){

  vectorForce.at(0) = msg.wrench.force.x;
  vectorForce.at(1) = msg.wrench.force.y;
  vectorForce.at(2) = msg.wrench.force.z;

  vectorTorque.at(0) = msg.wrench.torque.x;
  vectorTorque.at(1) = msg.wrench.torque.y;
  vectorTorque.at(2) = msg.wrench.torque.z;

}

void CoordInterfaceCoord::publishUpdatedGoal(Eigen::Matrix4d wTgoal){

//  geometry_msgs::Vector3Stamped linPosition;
//  linPosition.vector.x = wTgoal(0,3);
//  linPosition.vector.y = wTgoal(1,3);
//  linPosition.vector.z = wTgoal(2,3);

//  pubUpdateGoal.publish(linPosition);

  peg_msgs::transformMat transfMsg;
  transfMsg.linPosition.vector.x = wTgoal(0,3);
  transfMsg.linPosition.vector.y = wTgoal(1,3);
  transfMsg.linPosition.vector.z = wTgoal(2,3);

  //COLUMN MAJOR (column 1 .... column 3)
  transfMsg.rotation.resize(9);
  transfMsg.rotation.at(0).data = wTgoal(0,0);
  transfMsg.rotation.at(1).data = wTgoal(1,0);
  transfMsg.rotation.at(2).data = wTgoal(2,0);
  transfMsg.rotation.at(3).data = wTgoal(0,1);
  transfMsg.rotation.at(4).data = wTgoal(1,1);
  transfMsg.rotation.at(5).data = wTgoal(2,1);
  transfMsg.rotation.at(6).data = wTgoal(0,2);
  transfMsg.rotation.at(7).data = wTgoal(1,2);
  transfMsg.rotation.at(8).data = wTgoal(2,2);

  pubUpdateGoal.publish(transfMsg);

}