// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
ROS_INFO_STREAM("Received Position Command");
const geometry_msgs::PoseStamped* data = msg.get();
des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());
if(bOrientCtrl) {
tf::Quaternion q = des_ee_pose.getRotation();
ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
}
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example3" );
ros::NodeHandle n;
ros::Rate rate( 1 );
while( ros::ok() ) {
ROS_DEBUG_STREAM( "DEBUG message." );
ROS_INFO_STREAM ( "INFO message." );
ROS_WARN_STREAM ( "WARN message." );
ROS_ERROR_STREAM( "ERROR message." );
ROS_FATAL_STREAM( "FATAL message." );
ROS_INFO_STREAM_NAMED( "named_msg", "INFO named message." );
ROS_INFO_STREAM_THROTTLE( 2, "INFO throttle message." );
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
void GenericHWInterface::printState()
{
// WARNING: THIS IS NOT REALTIME SAFE
// FOR DEBUGGING ONLY, USE AT YOUR OWN ROBOT's RISK!
ROS_INFO_STREAM_THROTTLE(1, std::endl
<< printStateHelper());
}
void GlobalPlanner::QueryRobot(int id)
{
return;
global_planner::RobotStatusSrv s;
s.request.id = id;
if (m_statusServices[id])
{
if (m_statusServices[id].call(s))
{
m_robots[s.request.id]->SetData(s.response.status);
ROS_INFO_STREAM_THROTTLE(10.0, "Received response from robot: "<<s.response.status.id<<" : "<<m_robots[s.request.id]->ToString());
}
else
{
ROS_ERROR_STREAM("Did not receive good response from robot: "<<id);
}
}
else
{
ROS_ERROR_STREAM_THROTTLE(3.0, "Not connected to robot: "<<id<<"... retrying");
std::string serviceTopic = Conversion::RobotIDToServiceName(id);
m_statusServices[id] = m_nh->serviceClient<global_planner::RobotStatusSrv>(serviceTopic, false);
}
}
bool is_in_socket_area(arma::colvec3& x_bb, arma::colvec3& x_tt){
if(x_bb(1) > -0.05 && x_tt(1) < 0.05 && x_bb(2) > -0.05 && x_tt(2) < 0.05){
return true;
ROS_INFO_STREAM_THROTTLE(1.0," IS IN SOCKET AREA");
}else{
return false;
}
}
// Callback for the desired end effector force/torque
void ftCallback(const geometry_msgs::WrenchStampedConstPtr& msg) {
const geometry_msgs::WrenchStamped* data = msg.get();
des_ee_ft[0] = data->wrench.force.x;
des_ee_ft[1] = data->wrench.force.y;
des_ee_ft[2] = data->wrench.force.z;
des_ee_ft[3] = data->wrench.torque.x;
des_ee_ft[4] = data->wrench.torque.y;
des_ee_ft[5] = data->wrench.torque.z;
ROS_INFO_STREAM_THROTTLE(1, "Received FT: "<<des_ee_ft[0]<<","<<des_ee_ft[1]<<","<<des_ee_ft[2]<<","
<<des_ee_ft[3]<<","<<des_ee_ft[4]<<","<<des_ee_ft[5]);
}
/// @brief The event loop. Basically an ever running while with ros::spinOnce()
/// This is a re-implementation taking into care the memory scopes and also processing only points with higher image gradients
void ImuDeadReckon::eventLoop()
{
ros::Rate rate(100); //100Hz
while( ros::ok() )
{
ros::spinOnce();
if( !(this->isIMUDataReady) )
continue;
updateNominalStateWithCurrentMeasurements();
publishPose();
ROS_INFO_STREAM_THROTTLE( 5, "(every5 sec) Lin Acc : "<< lin_acc.transpose() );
ROS_INFO_STREAM_THROTTLE( 5, "Ang Vel : "<< ang_vel.transpose() );
rate.sleep();
}
}
/***********************************************************************
* Method: GlobalPlanner::PlanNNWaypoint
* Params:
* Returns:
* Effects: Iterate over available robots and choose the nearest available waypoint
***********************************************************************/
void GlobalPlanner::PlanNNWaypoint()
{
ROS_INFO_STREAM_THROTTLE(10,"Planning NN Waypoints...");
std::vector<Robot_Ptr> availableRobots = GetAvailableRobots(1); // Find those robots with at least 1 storage space
std::map<int, Waypoint_Ptr> allWaypoints = m_tm.GetWaypoints();
std::vector<Waypoint_Ptr> availableWaypoints = m_tm.GetAvailableWaypoints();
// Break if all waypoints reached.
if (availableWaypoints.size() == 0) { return; }
for (std::vector<Robot_Ptr>::iterator i = availableRobots.begin(); i != availableRobots.end(); ++i)
{
Robot_Ptr robot = *i;
if (robot->GetType() == RobotState::BIN_BOT) {
// TEMP: bin bots don't search waypoints..
continue;
}
// Get updated set of available waypoints each time
availableWaypoints = m_tm.GetAvailableWaypoints();
ROS_INFO_STREAM("Waypoints to go: (" << availableWaypoints.size() << ")");
// Break if all waypoints reached.
if (availableWaypoints.size() == 0) {
break;
}
int waypoint_id = GetWaypointClosestToRobot(robot->GetID());
if (waypoint_id == NO_WAYPOINT_FOUND) {
ROS_WARN("No Waypoint Found!");
break;
}
Waypoint_Ptr bestwp = allWaypoints[waypoint_id];
// Assign Robot to Waypoint
if (!AssignRobotWaypoint(robot->GetID() , waypoint_id))
{
ROS_ERROR_STREAM_THROTTLE(1, "Error Assigning Robot " << robot->GetName() << "(" << robot->GetID() << ")"
<< " - Waypoint(" << waypoint_id << ")");
}
// Print out waypoints and their statuses
for (std::map<int, Waypoint_Ptr>::iterator it = allWaypoints.begin(); it != allWaypoints.end(); ++it)
{
ROS_INFO_STREAM(it->second->ToShortString());
}
}
}
// Estimated FT on the end-effector coming from the state estimator
void estFTCallback(const geometry_msgs::WrenchStampedConstPtr& msg) {
const geometry_msgs::WrenchStamped* data = msg.get();
est_ee_ft[0] = data->wrench.force.x;
est_ee_ft[1] = data->wrench.force.y;
est_ee_ft[2] = data->wrench.force.z;
est_ee_ft[3] = data->wrench.torque.x;
est_ee_ft[4] = data->wrench.torque.y;
est_ee_ft[5] = data->wrench.torque.z;
est_ee_ft -= shift_ee_ft;
// est_ee_ft[5] = -1.0;
isFTOkay = true;
ROS_INFO_STREAM_THROTTLE(1, "Received FT: "<<des_ee_ft[0]<<","<<des_ee_ft[1]<<","<<des_ee_ft[2]<<","
<<des_ee_ft[3]<<","<<des_ee_ft[4]<<","<<des_ee_ft[5]);
}
void MotorController::recieveResponse()
{
/* Read response from the server */
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
memset(buffer, 0, sizeof(buffer));
/* read from the buffer */
n = (*sock_).readMessage(buffer); // blocks until data is read
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
bool status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
publishResponse(response);
ROS_INFO_STREAM_THROTTLE(log_period_, "Rate: " << 1 / response.dt_sec << "hz.");
}
/**
* \brief Fills data using XsDataPacket object
* @param _packet Incoming packet from Xsens device.
*/
void SensorData::fillData(XsDataPacket * _packet){
XsDataPacket packet = *_packet;
XsMessage msg = packet.toMessage();
XsSize msg_size = msg.getDataSize();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Data Size: " << msg_size);
// Get the orientation data
if (packet.containsOrientation()) {
XsQuaternion quaternion = packet.orientationQuaternion();
q1 = quaternion.m_x;
q2 = quaternion.m_y;
q3 = quaternion.m_z;
q0 = quaternion.m_w;
XsEuler euler = packet.orientationEuler();
eroll = euler.m_roll;
epitch = euler.m_pitch;
eyaw = euler.m_yaw;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,"Orientation: Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll
<< ", Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch
<< ", Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw);
}
// Get the gyroscope data
if (packet.containsCalibratedGyroscopeData()) {
XsVector gyroscope = packet.calibratedGyroscopeData();
gyrX = gyroscope.at(0);
gyrY = gyroscope.at(1);
gyrZ = gyroscope.at(2);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Angular Velocity: ( "
<< gyrX << ", " << gyrY << ", " << gyrZ << ")" );
}
// Get the acceleration data
if (packet.containsCalibratedAcceleration()) {
XsVector acceleration = packet.calibratedAcceleration();
accX = acceleration.at(0);
accY = acceleration.at(1);
accZ = acceleration.at(2);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Linear Acceleration: ("
<< accX << "," << accX << "," << accZ << ")" );
}
//Get the altitude data
if(packet.containsAltitude()){
mAltitude = packet.altitude();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsAltitude: ");
}
//Get the Latitude and Longitude Data
if(packet.containsLatitudeLongitude()){
XsVector latlon = packet.latitudeLongitude();
mLatitude = latlon[0];
mLongitude = latlon[1];
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsLatitudeLongitude");
}
// Get the magnetic field data
if (packet.containsCalibratedMagneticField()) {
XsVector magneticField = packet.calibratedMagneticField();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Magnetic Field: ("<< magneticField[0] <<" , "
<< magneticField[1] << " , "<< magneticField[2] << ")");
magX = magneticField.at(0);
magY = magneticField.at(1);
magZ = magneticField.at(2);
}
// Get Temperature data
if (packet.containsTemperature()){
double temperature = packet.temperature();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Temperature : " << temperature);
mTemperature = temperature;
}
// Get Pressure Data
if(packet.containsPressure() ){
XsPressure pressure = packet.pressure();
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pressure : " << pressure.m_pressure);
mPressure= pressure.m_pressure;
}
// Get Velocity Data
if( packet.containsVelocity() ){
XsVector velocity = packet.velocity();
mVelocityX = velocity.at(0);
mVelocityY = velocity.at(1);
mVelocityZ = velocity.at(2);
ROS_INFO_STREAM(" Velocity [ x (east) : " << mVelocityX << ", y (north) : "
<< mVelocityY << ", z (up) " << mVelocityZ << " ]");
}
// Get GPS PVT Data
if( packet.containsGpsPvtData() ){
XsGpsPvtData gpsPvtData = packet.gpsPvtData();
ROS_INFO_STREAM(" Horizontal accuracy: " << gpsPvtData.m_hacc );
}
if(packet.containsRawAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Acceleration") ;}
if(packet.containsRawGyroscopeData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Gyroscope") ;}
if(packet.containsRawGyroscopeTemperatureData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawGyrTemp");}
if(packet.containsRawMagneticField() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawMag");}
if(packet.containsRawTemperature() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawTemp");}
if(packet.containsRawData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawData");}
if(packet.containsCalibratedData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Calib Data");}
if(packet.containsSdiData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SDI");}
//if(packet.containsStatusByte() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "StatusByte");}
if(packet.containsDetailedStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "DetailedStatus");}
if(packet.containsPacketCounter8() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter8");}
if(packet.containsPacketCounter() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter");}
if(packet.containsSampleTimeFine() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeFine");}
if(packet.containsSampleTimeCoarse() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeCoarse");}
if(packet.containsSampleTime64() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTime64");}
if(packet.containsFreeAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "FreeAcceleration");}
if(packet.containsPressureAge() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PressureAge");}
if(packet.containsAnalogIn1Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN1IN");}
if(packet.containsAnalogIn2Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN2IN");}
if(packet.containsPositionLLA() ){
XsVector posLLA = packet.positionLLA();
mLatitude = posLLA[0];
mLongitude = posLLA[1];
mAltitude = posLLA[2];
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLatitude = " << mLatitude);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLongitude = " << mLongitude);
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mAltitude = " << mAltitude);
//for(int i=0; i < posLLA.size(); i++){
//ROS_INFO_STREAM("posLLA[" << i << "] : " << posLLA[i] );
//}
}
if(packet.containsUtcTime() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "UTC-Time");}
if(packet.containsFrameRange() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Frame Range");}
if(packet.containsRssi() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RSSI");}
if(packet.containsRawGpsDop() ){
//ROS_INFO_THROTTLE(THROTTLE_VALUE, "DOP");}
XsRawGpsDop dop = packet.rawGpsDop();
m_gdop = ((float) dop.m_gdop)/100;
m_pdop = ((float) dop.m_pdop)/100;
m_tdop = ((float) dop.m_tdop)/100;
m_vdop = ((float) dop.m_vdop)/100;
m_hdop = ((float) dop.m_hdop)/100;
m_edop = ((float) dop.m_edop)/100;
m_ndop = ((float) dop.m_ndop)/100;
m_itow = ((float) dop.m_itow)/1000;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,
"GDOP = " << m_gdop <<
",PDOP = " << m_pdop <<
",TDOP = " << m_tdop <<
",VDOP = " << m_vdop <<
",HDOP = " << m_hdop <<
",EDOP = " << m_edop <<
",NDOP = " << m_ndop <<
",ITOW = " << m_itow);
}
if(packet.containsRawGpsSol() ){
// ROS_INFO_THROTTLE(THROTTLE_VALUE, "SOL");}
XsRawGpsSol sol = packet.rawGpsSol();
mPositionAccuracy = sol.m_pacc;
mSpeedAccuracy = sol.m_sacc;
mSatelliteNumber = sol.m_numsv;
mGpsFixStatus = sol.m_gpsfix;
gpsVelocityX=sol.m_ecef_vx/100;
gpsVelocityY=sol.m_ecef_vy/100;
gpsVelocityZ=sol.m_ecef_vz/100;
ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,
"Pos Acc = " << mPositionAccuracy <<
",Speed Acc = " << mSpeedAccuracy <<
",Sat Number = " << mSatelliteNumber <<
",GPS FIX = " << std::hex << mGpsFixStatus);
}
if(packet.containsRawGpsTimeUtc() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "GPS-UTC");}
if(packet.containsRawGpsSvInfo() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SV INFO");}
// if(packet.containsTriggerIndication() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "TRIGGER");}
//Get Status byte
if( packet.containsStatus() ){
mStatus = packet.status();
ROS_INFO_THROTTLE(10, "Status: %.8X", mStatus);
}
}
// Executive function
void GlobalPlanner::Execute()
{
ros::spinOnce();
if ((ros::Time::now() - m_lastDisplay) > ros::Duration(5))
{
Display();
}
// Get Robot Status...
QueryRobots();
// FOR each pair of robots that're just chillin in a dump stage and are stopped near the handoff location
// Pick best bot to meet with?
// Check if robot1 & robot2 are within handoff threshold
// Add new dump to m_dumpMap
// Send handoff message to each robot giving them their new capacities
//
// IF robot in wait state AND is full AND there are goals
// Find the best dump location for the robots to meet
// Update Dump List
// Send Dump Message
//
// IF there are goals available
// pick best robot to do job (return an ID)
// OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to
// Send goal message
// ELSE IF robots available AND waypoints available
// pick best robot to get to waypoints (return an ID)
// OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to
// Send Waypoint Message
// Check if a robot is full & find the best binbot for it
std::vector<Robot_Ptr> availableRobots = GetAvailableRobots(0); // Get any available robots even with no storage space left
for (std::vector<Robot_Ptr>::iterator it = availableRobots.begin(); it != availableRobots.end(); ++it)
{
Robot_Ptr robot = *it;
// If robot has no space (TODO: Check if collector bot or bin bot for where to dump)
if (robot->GetStorageAvailable() <= 0 &&
robot->GetType() == RobotState::COLLECTOR_BOT)
{
ROS_INFO_STREAM_THROTTLE(5, "Robot " << robot->GetName() <<
"(" << robot->GetID() << ")" <<
" full, searching for closest available bin bot...");
int collectorBot = robot->GetID();
// Get closest bin bot to collector bot if it exists
int bestBinBot = GetBestBinBot( collectorBot );
if (bestBinBot != NO_ROBOT_FOUND) {
ROS_INFO_STREAM("Bin Bot " << robot->GetName() <<
"(" << robot->GetID() << ") found.");
// Create new dump site
Dump_Ptr dp(new DumpWrapper(dumps_count++));
m_tm.AddDump(dp);
// Assign collector robot to dump, Assign bin bot to dump
if (!AssignRobotsDump(collectorBot, bestBinBot, dp->GetID()))
{
ROS_ERROR_STREAM("Error Assigning Robots " << m_robots[collectorBot]->GetName() <<
"(" << collectorBot << ") & " << m_robots[bestBinBot]->GetName() <<
"(" << bestBinBot << ")" << " to Dump(" << dp->GetID() << ")");
}
}
}
}
// Check each dump, see if each robot in the dump finished state
std::map<int, Dump_Ptr > dumps = m_tm.GetDumps();
for (std::map<int, Dump_Ptr>::iterator it = dumps.begin(); it != dumps.end(); ++it)
{
Dump_Ptr dump = it->second;
int collectorBot = dump->GetRobot1();
int binBot = dump->GetRobot2();
// if (dump->GetReadyToTransfer()) { // This requires services to work
// Avoid services by querying robot states
if ( dump->GetInProgress() && (m_robots[collectorBot]->GetState() == RobotState::WAITING || m_robots[binBot]->GetState() == RobotState::WAITING) )
{
// Resend dump so waiting robot
m_tm.SendDump(dump->GetID());
}
if (dump->GetInProgress() && m_robots[collectorBot]->GetState() == RobotState::DUMPING_FINISHED && m_robots[binBot]->GetState() == RobotState::DUMPING_FINISHED)
{
ROS_INFO_STREAM("Dump(" << dump->GetID() << ") transferring trash.");
// Move trash over
int collectorBot = dump->GetRobot1();
int binBot = dump->GetRobot2();
int trash_to_transfer = m_robots[collectorBot]->GetStorageUsed();
// Send trash updates to robot controllers
global_planner::SetTrashSrv s;
s.request.storage = 0;
if (m_setTrashServices[collectorBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[collectorBot]->GetName()); }
else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << collectorBot); }
s.request.storage = m_robots[binBot]->GetStorageUsed() + trash_to_transfer;
if (m_setTrashServices[binBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[binBot]->GetName()); }
else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << binBot); }
// In global planner, update storage values
// Add trash to bin bot
m_robots[binBot]->SetStorageUsed( m_robots[binBot]->GetStorageUsed() + trash_to_transfer );
// Remove trash from collector bot
m_robots[collectorBot]->SetStorageUsed(0);
// Set states to waiting in global planner but not robot controllers
// the robot controllers are in DUMP_FINISHED state, but they're ready to transition to waypoints etc.
// So set them to waiting in global planner and hopefully go to planning before a callback update resets them.
m_robots[collectorBot]->SetState(RobotState::WAITING);
m_robots[binBot]->SetState(RobotState::WAITING);
// Transition dump state to success
dump->SetStatus(TaskResult::SUCCESS);
}
}
// ProcessGoals();
/*
// If no available waypoints, do nothing
if (m_tm.GetAvailableWaypoints().size() == 0) {
ROS_WARN_STREAM_THROTTLE(10, "No Available Waypoints! ("
<< m_tm.GetAvailableWaypoints().size() << ") "
<< (isFinished() ? "FIN" : "GP Waiting") );
// Print out status of all waypoints
std::map<int, Waypoint_Ptr> allWaypoints = m_tm.GetWaypoints();
std::stringstream ss;
for (std::map<int, Waypoint_Ptr>::iterator it = allWaypoints.begin(); it != allWaypoints.end(); ++it)
{
ss << it->second->GetID() << "-" << it->second->GetStatusMessage() << " ";
}
ROS_INFO_STREAM_THROTTLE(10, ss.str() );
return;
}
*/
switch (m_planner)
{
case PLANNER_CLOSEST_ROBOT:
PlanNNRobot();
break;
case PLANNER_CLOSEST_WAYPOINT:
PlanNNWaypoint();
break;
case PLANNER_NAIVE:
default:
PlanNaive();
break;
}
}
int main(int argc,char** argv){
std::map<std::string,std::string> input;
input["-ft_sensor_topic"] = "";
input["-virtual_sensor_topic"] = "";
input["-action_topic"] = "";
input["-urdf"] = "";
input["-rate"] = "100";
input["-fixed_frame"] = "/world";
input["-peg_link_name"] = "";
input["-path_sensor_model"] = "";
input["-socket_type"] = "";
if(!opti_rviz::Input::process_input(argc,argv,input)){
ROS_ERROR("failed to load input");
return 0;
}
opti_rviz::Input::print_input_options(input);
std::string ft_sensor_topic = input["-ft_sensor_topic"];
std::string virtual_sensor_topic = input["-virtual_sensor_topic"];
std::string action_topic = input["-action_topic"];
std::string fixed_frame = input["-fixed_frame"];
std::string peg_link_name = input["-peg_link_name"];
std::string srate = input["-rate"];
std::string path_sensor_model = input["-path_sensor_model"];
std::string ssocket_type = input["-socket_type"];
SOCKET_TYPE socket_type;
if(ssocket_type == "one"){
socket_type = SOCKET_TYPE::ONE;
}else if(ssocket_type == "two"){
socket_type = SOCKET_TYPE::TWO;
}else if(ssocket_type == "three"){
socket_type = SOCKET_TYPE::THREE;
}else{
ROS_ERROR_STREAM("No such socket type defined: " + ssocket_type);
return 0;
}
ros::init(argc, argv, "peg_filter");
ros::NodeHandle nh;
double Hz = boost::lexical_cast<float>(srate);
ros::Rate rate(Hz);
ROS_INFO_STREAM("loaded variables! [peg_filter_node]");
/// ---------------------- Variables -----------------------------
arma::colvec3 peg_origin;
arma::mat33 peg_orient;
/// ------------------ Virtual Sensor for Particles -------------------------
Peg_world_wrapper peg_world_wrapper(nh,socket_type,true,"peg_world_wrapper",path_sensor_model,fixed_frame,peg_link_name); // don't publish needs the model for the likelihood
psm::Contact_distance_model contact_distance_model(*(peg_world_wrapper.peg_sensor_model.get()));
psm::Sensor_manager sensor_manager(nh);
sensor_manager.add("contact",&contact_distance_model);
if(!sensor_manager.select_model("contact")){
ROS_ERROR("FAILED to select_model");
exit(0);
}
/// ------------------ Sensor Variables -------------------------
psm::Peg_sensor_listener ft_sensor_listener(nh,ft_sensor_topic);
psm::Peg_sensor_listener virtual_sensor_listener(nh,virtual_sensor_topic);
arma::colvec& Y_ft = ft_sensor_listener.Y;
arma::colvec& Y_virtual = virtual_sensor_listener.Y;
arma::colvec Y_mixed;
int Y_type = 3;
Y_mixed.resize(10);
opti_rviz::Publisher pub_mix(nh,"mixed_classifier");
/// ------------------ Point Mass Filter -------------------------
///
///
///
///
bool bExperiment = true;
// Debug
int init_pmf_type = 10;
int init_pmf_pos = 1;
// experiment
int init_pmf_exp = 1;
int init_exp_pos = 1;
///
///
/// Dimension of Measurement
int dim_Y = 12;
//likeli::Binary_likelihood binary_likelihood;
likeli::Mixed_likelihood mixed_likelihood(nh,peg_world_wrapper.get_wrapped_objects(),peg_origin);
pf::Measurement_h peg_measurement = std::bind(&psm::Sensor_manager::compute_hY,&sensor_manager,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3);
pf::likelihood_model peg_likelihood;
// the type of likelihood function will depend on the sensor (FT/virtual)
if(Y_type == 0)
{
// peg_likelihood = std::bind(&likeli::Binary_likelihood::likelihood,&binary_likelihood,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3);
}else if(Y_type == 2)
{
peg_likelihood = std::bind(&likeli::Mixed_likelihood::likelihood,&mixed_likelihood,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);
}else if (Y_type == 3)
{
if(socket_type == SOCKET_TYPE::ONE){
ROS_WARN("SETTING LIKELIHOOD TO SOCKET ONE");
peg_likelihood = std::bind(&likeli::Mixed_likelihood::likelihood,&mixed_likelihood,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);
}else if(socket_type == SOCKET_TYPE::TWO){
ROS_WARN("SETTING LIKELIHOOD TO SOCKET TWO");
peg_likelihood = std::bind(&likeli::Mixed_likelihood::likelihood_stwo,&mixed_likelihood,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);
}else if(socket_type == SOCKET_TYPE::THREE){
ROS_WARN("SETTING LIKELIHOOD TO SOCKET THREE");
peg_likelihood = std::bind(&likeli::Mixed_likelihood::likelihood_sthree,&mixed_likelihood,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);
}else{
ROS_ERROR("PEG FILTER NO SUCH SOCKET TYPE");
}
}else{
ROS_ERROR_STREAM("No such [" << Y_type << "] defined!");
exit(0);
}
pf::Point_mass_filter::delta delta_;
pf::Point_mass_filter::length length_;
if(bExperiment){
init_delta_length_experiment(init_pmf_exp,delta_,length_);
}else{
init_delta_length(init_pmf_type,delta_,length_);
}
pf::Point_mass_filter pmf(peg_likelihood,peg_measurement,delta_,length_,dim_Y);
arma::colvec3 peg_position;
{
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once(fixed_frame,peg_link_name,transform);
opti_rviz::type_conv::tf2vec(transform.getOrigin(),peg_position);
}
arma::colvec3 table_position;
{
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once(fixed_frame,"link_wall",transform);
opti_rviz::type_conv::tf2vec(transform.getOrigin(),table_position);
}
arma::colvec3 socket_position;
{
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once(fixed_frame,"link_socket",transform);
opti_rviz::type_conv::tf2vec(transform.getOrigin(),socket_position);
}
if(bExperiment){
initialise_prior_experiment(init_pmf_exp,pmf,peg_position(0),peg_position(1),peg_position(2));
}else{
initialise_prior_pdf(init_pmf_pos,init_pmf_type,pmf,peg_position(0),peg_position(1),peg_position(2));
}
std::cout<< "pmf prior initialised [peg_filter_node]" << std::endl;
//binary_likelihood.set_resolution(&pmf.get_delta());
mixed_likelihood.set_resolution(&pmf.get_delta());
std::cout<< "binary_likelihood set [peg_filter_node]" << std::endl;
plugfilter::Plug_pf_manager plug_pf_manager;
plug_pf_manager.add(&pmf,"pmf");
if(!plug_pf_manager.select_method("pmf")){
ROS_ERROR("did not manage to select pmf [peg_filter_node]");
}
plug_pf_manager.init_visualise(nh);
plug_pf_manager.set_pf_color_type(pf::C_WEIGHTS);
plug_pf_manager.set_visualise_mode(opti_rviz::Vis_point_cloud::DEFAULT);
/// ------------------- Filter Manager Service ---------------------
plugfilter::Plug_service plug_service(nh,plug_pf_manager);
plugfilter::Plug_service::Initialise initialise_f = std::bind(initialise_prior_pdf,init_pmf_pos,init_pmf_type,std::ref(pmf),std::placeholders::_1,std::placeholders::_2,std::placeholders::_3);
plugfilter::Plug_service::Initialise initialise_ex = std::bind(initialise_prior_experiment,init_pmf_exp,std::ref(pmf),std::placeholders::_1,std::placeholders::_2,std::placeholders::_3);
if(bExperiment){
plug_service.set_initilisation_function(&initialise_ex);
}else{
plug_service.set_initilisation_function(&initialise_f);
}
std::cout<< "Plug_service set [peg_filter_node]" << std::endl;
/// Listeners Velocity
opti_rviz::Listener peg_tip_position_listener(fixed_frame,action_topic);
tf::Vector3 peg_origin_tf;
tf::Vector3 peg_origin_tf_tmp;
tf::Matrix3x3 peg_orient_tf;
peg_tip_position_listener.update(peg_origin_tf,peg_orient_tf);
peg_origin_tf_tmp = peg_origin_tf;
/// ------------------ Belief Compression ------------------
Belief_compression belief_compression;
// advertised on topic [mode_feature]
ModeBeliefCompress mode_belief_compression(nh,"mode_entropy");
belief_compression.add_bel_compress_method(&mode_belief_compression);
if(belief_compression.set_method("mode_entropy"))
{
ROS_INFO("belief compression set [peg_filter_node]");
}else{
ROS_ERROR("belief compression not set [peg_filter_node]");
exit(0);
}
/// ------------ Get Fist time Position and Orientation ------------
tf::Matrix3x3 Rot_peg;
Rot_peg.setRPY(0,0,M_PI);
arma::colvec3 u;
peg_orient.zeros();
peg_origin.zeros();
bool bcorrect_orient = false;
peg_origin_tf_tmp = peg_origin_tf;
while(nh.ok() && (bcorrect_orient == false))
{
peg_tip_position_listener.update(peg_origin_tf,peg_orient_tf);
opti_rviz::type_conv::tf2mat(peg_orient_tf,peg_orient);
opti_rviz::type_conv::tf2vec(peg_origin_tf,peg_origin);
bcorrect_orient = arma::any(arma::vectorise(peg_orient));
ros::spinOnce();
rate.sleep();
}
peg_origin_tf_tmp = peg_origin_tf;
get_veclocity(u,peg_origin_tf,peg_origin_tf_tmp);
u.print("u");
peg_orient.print("peg_orient");
/// ------------ Remove Bias from FT sensor --------------
ros::ServiceClient client = nh.serviceClient<netft_rdt_driver::String_cmd>("/ft_sensor/bias_cmd");
ros::Subscriber sub_ft_bias_status = nh.subscribe("/ft_sensor/bias_status", 10, bias_status_callback);
netft_rdt_driver::String_cmd srv;
srv.request.cmd = "bias";
srv.response.res = "";
if (client.call(srv))
{
ROS_INFO_STREAM("net_ft res: " << srv.response.res);
}
else
{
ROS_ERROR("Failed to call netft bias service [peg_filter_node]");
}
while(nh.ok() && !bBiasUpdated)
{
ros::spinOnce();
rate.sleep();
}
ROS_INFO("bias reset");
/// ------------ Wait a litte bit ------------
{
while(nh.ok() && Y_ft(0) != 0){
std::cout << "waiting all zero Y: " << Y_ft(0) << " " << Y_ft(1) << " " << Y_ft(2) << std::endl;
ros::spinOnce();
rate.sleep();
}
}
tf::Quaternion qtmp;
qtmp.setX(0);
qtmp.setY(-0.5);
qtmp.setZ(0);
qtmp.setW(0.5);
arma::colvec3 wall_position;
{
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once("world","link_wall",transform);
opti_rviz::type_conv::tf2vec(transform.getOrigin(),wall_position);
}
opti_rviz::Vis_point_cloud vis_pc_rec(nh,"pfilter_record");
vis_pc_rec.set_channel(opti_rviz::Vis_point_cloud::CHANNEL_TYPE::Intensity);
vis_pc_rec.initialise("world",pmf.points);
arma::colvec3 plan_norm;
plan_norm(0) = 1; plan_norm(1) = 0; plan_norm(2) = 0;
arma::colvec3 target,mode_SF, mode, tt, bb;
target = table_position;
target(0) = target(0) + 0.026;
ros::Time start_time = ros::Time::now();
while(nh.ok()){
// const auto start_time = std::chrono::steady_clock::now();
peg_world_wrapper.update();
peg_tip_position_listener.update(peg_origin_tf,peg_orient_tf);
get_veclocity(u,peg_origin_tf,peg_origin_tf_tmp);
peg_orient_tf.setRotation(qtmp);
opti_rviz::type_conv::tf2mat(peg_orient_tf,peg_orient);
opti_rviz::type_conv::tf2vec(peg_origin_tf,peg_origin);
bcorrect_orient = arma::any(arma::vectorise(peg_orient));
mode = mode_belief_compression.get_mode();
bb = pmf.bbox.bb - socket_position;
tt = pmf.bbox.tt - socket_position;
mode_SF = mode - socket_position;
if(!(Y_ft.is_empty()) && bcorrect_orient){
opti_rviz::debug::tf_debuf(bb,"BB");
opti_rviz::debug::tf_debuf(tt,"TT");
if(Y_ft(0) == 1 && is_above_table(bb) && is_above_table(tt) && !is_in_socket_area(bb,tt)){
// ROS_INFO_STREAM_THROTTLE(0.2,"----------------> Is above table");
if(mode_SF.is_finite()){
// u(0) = 0.01 * (0.02 - mode_SF(0));
}else{
ROS_INFO_STREAM_THROTTLE(1.0, "MODE_SF IS NOT FINITE");
}
}else if(Y_ft(0) == 1 && !Y_virtual(psm::Contact_distance_model::C_SOCKET))
{
// u(0) = 0;
}
switch (Y_type) {
case 0:
{
plug_pf_manager.update(Y_ft,u,peg_orient,Hz);
break;
}
case 1:
{
plug_pf_manager.update(Y_virtual,u,peg_orient,Hz);
break;
}
case 2:
{
Y_mixed(0) = Y_ft(0); // contact / no contact
Y_mixed(1) = Y_ft(1); // Fx
Y_mixed(2) = Y_ft(2); // Fy
Y_mixed(3) = Y_ft(3); // Fz
Y_mixed(4) = Y_ft(4); // prob_left_edge
Y_mixed(5) = Y_ft(5); // prob_right_edge
Y_mixed(6) = Y_ft(6); // prob_up_edge
Y_mixed(7) = Y_ft(7); // prob_down_edge
pub_mix.publish(Y_mixed);
plug_pf_manager.update(Y_mixed,u,peg_orient,Hz);
break;
}
case 3:
{
/*
C_SURF =0
C_EDGE_DIST =1
C_EDGE_LEFT =2
C_EDGE_RIGHT =3
C_EDGE_TOP =4
C_EDGE_BOT =5
C_RING =6
C_S_HOLE =7
C_SOCKET =8
C_EDGE_V1 =9
C_EDGE_V2 =10
C_EDGE_V3 =11
*/
Y_mixed(0) = Y_ft(0); // contact / no contact
Y_mixed(1) = Y_virtual(psm::Contact_distance_model::C_EDGE_DIST); // Fx
Y_mixed(2) = Y_virtual(psm::Contact_distance_model::C_EDGE_V2); // Fy
Y_mixed(3) = Y_virtual(psm::Contact_distance_model::C_EDGE_V3); // Fz
Y_mixed(4) = Y_virtual(psm::Contact_distance_model::C_EDGE_LEFT); // prob_left_edge
Y_mixed(5) = Y_virtual(psm::Contact_distance_model::C_EDGE_RIGHT); // prob_right_edge
Y_mixed(6) = Y_virtual(psm::Contact_distance_model::C_EDGE_TOP); // prob_up_edge
Y_mixed(7) = Y_virtual(psm::Contact_distance_model::C_EDGE_BOT); // prob_down_edge
Y_mixed(8) = Y_virtual(psm::Contact_distance_model::C_SOCKET); // Y_socket
Y_mixed(9) = Y_virtual(psm::Contact_distance_model::C_RING); // Closest distance to ring edge
pub_mix.publish(Y_mixed);
plug_pf_manager.update(Y_mixed,u,peg_orient,Hz);
break;
}
default:
break;
}
/// belief feature (belief compression) computation
belief_compression.update(u,pmf.points,pmf.P);
}
/// particle filter visualisation
plug_pf_manager.visualise();
/// particle filter vis for recording
if( (ros::Time::now() - start_time).toSec() > 0.02 ){
vis_pc_rec.update(pmf.points,pmf.P.memptr());
vis_pc_rec.publish();
start_time = ros::Time::now();
}
peg_origin_tf_tmp = peg_origin_tf;
ros::spinOnce();
rate.sleep();
}
return 0;
}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
// --------- Print function -------------------------------------------------------------
void pose_controller::print(){
float sec = 0.12;
ROS_INFO_STREAM_THROTTLE(sec, "--------------ODOMETRY CONTROL--------------");
ROS_INFO_STREAM_THROTTLE(sec, "Code state: " << status);
ROS_INFO_STREAM_THROTTLE(sec, "Battery: " << battery << "%");
ROS_INFO_STREAM_THROTTLE(sec, "Drone state: " << drone);
ROS_INFO_STREAM_THROTTLE(sec, "Altitude: " << altd);
ROS_INFO_STREAM_THROTTLE(sec, "Info: " << info);
ROS_INFO_STREAM_THROTTLE(sec, "------------------CONTROL------------------");
ROS_INFO_STREAM_THROTTLE(sec, " Velocities");
ROS_INFO_STREAM_THROTTLE(sec, " ----------");
ROS_INFO_STREAM_THROTTLE(sec, "Velocity x: " << msg_move.linear.x);
ROS_INFO_STREAM_THROTTLE(sec, "Velocity y: " << msg_move.linear.y);
ROS_INFO_STREAM_THROTTLE(sec, "Velocity z: " << msg_move.linear.z);
ROS_INFO_STREAM_THROTTLE(sec, "Yaw: " << msg_move.angular.z);
ROS_INFO_STREAM_THROTTLE(sec, "-------------------------------------------" << std::endl);
ROS_INFO_STREAM_THROTTLE(sec, std::endl);
}
void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovariance& msg)
{
// Construct pose position and orientation.
const geometry_msgs::Point& p = msg.pose.position;
const geometry_msgs::Quaternion& q = msg.pose.orientation;
Ogre::Vector3 position(p.x, p.y, p.z);
Ogre::Quaternion orientation(q.w, q.x, q.y, q.z);
// Set position and orientation for axes scene node.
if(!position.isNaN())
axes_->setPosition(position);
else
ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position);
if(!orientation.isNaN())
axes_->setOrientation (orientation);
else
ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation);
// check for NaN in covariance
for (unsigned i = 0; i < 3; ++i)
{
if(isnan(msg.covariance[i]))
{
ROS_WARN_THROTTLE(1, "covariance contains NaN");
return;
}
}
// Compute eigen values and vectors for both shapes.
std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 0));
std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 3));
Ogre::Quaternion positionQuaternion(computeRotation(msg, positionEigenVectorsAndValues));
Ogre::Quaternion orientationQuaternion(computeRotation(msg, orientationEigenVectorsAndValues));
positionNode_->setOrientation(positionQuaternion);
orientationNode_->setOrientation(orientationQuaternion);
// Compute scaling.
//Ogre::Vector3 axesScaling(1, 1, 1);
//axesScaling *= scaleFactor_;
Ogre::Vector3 positionScaling
(std::sqrt (positionEigenVectorsAndValues.second[0]),
std::sqrt (positionEigenVectorsAndValues.second[1]),
std::sqrt (positionEigenVectorsAndValues.second[2]));
positionScaling *= scaleFactor_;
Ogre::Vector3 orientationScaling
(std::sqrt (orientationEigenVectorsAndValues.second[0]),
std::sqrt (orientationEigenVectorsAndValues.second[1]),
std::sqrt (orientationEigenVectorsAndValues.second[2]));
orientationScaling *= scaleFactor_;
// Set the scaling.
/*if(!axesScaling.isNaN())
axes_->setScale(axesScaling);
else
ROS_WARN_STREAM("axesScaling contains NaN: " << axesScaling);*/
if(!positionScaling.isNaN())
positionNode_->setScale(positionScaling);
else
ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling);
if(!orientationScaling.isNaN())
orientationNode_->setScale(orientationScaling);
else
ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling);
// Debugging.
ROS_INFO_STREAM_THROTTLE
(1.,
"Position:\n"
<< position << "\n"
<< "Positional part 3x3 eigen values:\n"
<< positionEigenVectorsAndValues.second << "\n"
<< "Positional part 3x3 eigen vectors:\n"
<< positionEigenVectorsAndValues.first << "\n"
<< "Sphere orientation:\n"
<< positionQuaternion << "\n"
<< positionQuaternion.getRoll () << " "
<< positionQuaternion.getPitch () << " "
<< positionQuaternion.getYaw () << "\n"
<< "Sphere scaling:\n"
<< positionScaling << "\n"
<< "Rotational part 3x3 eigen values:\n"
<< orientationEigenVectorsAndValues.second << "\n"
<< "Rotational part 3x3 eigen vectors:\n"
<< orientationEigenVectorsAndValues.first << "\n"
<< "Cone orientation:\n"
<< orientationQuaternion << "\n"
<< orientationQuaternion.getRoll () << " "
<< orientationQuaternion.getPitch () << " "
<< orientationQuaternion.getYaw () << "\n"
<< "Cone scaling:\n"
<< orientationScaling
);
}
/***********************************************************************
* Method: RobotController::StateExecute
* Params: void *args
* Returns: void
* Effects: specifies what to run while in a state, and transition if
* appropriate
***********************************************************************/
void RobotController::StateExecute()
{
// WHILE in WAITING:
// IF received a waypoint: transition(NAVIGATING)
// IF received a dump message: transition(DUMPING)
// WHILE in NAVIGATING (waypoint navigation):
// IF received a (different) waypoint: change the pose to the new one
// IF received a dump message: transition(DUMPING)
// IF received a stop/cancel/estop: send waypoint result message (forced_stop) -> transition(WAITING)
// IF reached final pose of the waypoint, send waypoint result message (succeed) -> transition(WAITING)
// WHILE in DUMPING (going to dump):
// IF received a (different) waypoint: change the pose to the new one
// IF received a dump message: transition(DUMPING)
// IF received a stop/cancel/estop: send waypoint result message (forced_stop) -> transition(WAITING)
// IF reached final pose of the waypoint, send waypoint result message (succeed) -> transition(WAITING)
// ...
bool execResult = false;
// ROS_INFO_STREAM_THROTTLE(1.0, "STATE: "<<RobotState::ToString(m_status.GetState()));
switch(m_status.GetState())
{
case RobotState::WAITING:
if (m_tagProcessor->ShouldPause())
{
SendText("should pause - Waiting");
// SendSound("mario_pause.wav");
Transition(RobotState::WAITING_TAG_SPOTTED);
}
break;
//In this state, the robot should now be stopping and getting a more accurate view of the tag
case RobotState::WAITING_TAG_SPOTTED:
ros::spinOnce();
execResult = m_tagProcessor->Execute();
// ROS_DEBUG_STREAM_THROTTLE(0.5, "Result from execute: "<<execResult);
if (m_tagProcessor->ShouldResume())
{
//Transition back to the navigating state, using the same goal as before
SendText("should resume - WAITING_TAG_SPOTTED");
ROS_INFO("Resuming robot");
Transition(RobotState::WAITING_TAG_FINISHED);
break;
}
else
{
ROS_INFO_STREAM_THROTTLE(1, "Waiting on the OK to resume from the tag processor");
}
if (ros::Time::now() - m_timeEnteringState > ros::Duration(5))
{
ROS_ERROR_STREAM("ERROR: could not find any tags while stopped. we are going to just resume WAITING");
Transition(RobotState::NAVIGATING);
}
break;
case RobotState::WAITING_TAG_FINISHED:
if (ros::Time::now() - m_timeEnteringState > ros::Duration(1.0))
{
SendSound("mario_pause.wav");
SendText("resuming - WAITING_TAG_FINISHED");
Transition(RobotState::WAITING);
m_tagProcessor->SetShouldPause(false);
}
break;
case RobotState::NAVIGATING:
if (m_tagProcessor->ShouldPause())
{
Transition(RobotState::NAVIGATING_TAG_SPOTTED);
// SendSound("mario_pause.wav");
}
else
{
actionlib::SimpleClientGoalState::StateEnum result = action_client_ptr->getState().state_;
switch (result)
{
case actionlib::SimpleClientGoalState::SUCCEEDED:
ROS_INFO_STREAM("Movebase reached target (task id = "<<m_status.GetTaskID()<<")");
SendWaypointFinished(TaskResult::SUCCESS);
if (m_status.GetTaskID() < WAYPOINT_START_ID)
{
m_status.IncrementStorageUsed();
}
Transition(RobotState::WAITING);
break;
case actionlib::SimpleClientGoalState::ABORTED:
case actionlib::SimpleClientGoalState::REJECTED:
case actionlib::SimpleClientGoalState::LOST:
case actionlib::SimpleClientGoalState::RECALLED:
case actionlib::SimpleClientGoalState::PREEMPTED:
ROS_ERROR_STREAM("Navigation Failed: " << action_client_ptr->getState().toString() );
SendWaypointFinished(TaskResult::FAILURE);
Transition(RobotState::WAITING);
break;
case actionlib::SimpleClientGoalState::ACTIVE:
case actionlib::SimpleClientGoalState::PENDING:
default:
ROS_INFO_STREAM_THROTTLE(10, "Navigation state = " << action_client_ptr->getState().toString() );
break;
}
}
break;
//In this state, the robot should now be stopping and getting a more accurate view of the tag
case RobotState::NAVIGATING_TAG_SPOTTED:
ros::spinOnce();
execResult = m_tagProcessor->Execute();
// ROS_DEBUG_STREAM_THROTTLE(0.5, "Result from execute: "<<execResult);
if (m_tagProcessor->ShouldResume())
{
//Transition back to the navigating state, using the same goal as before
SendText("Resuming robot");
ROS_INFO("Resuming robot");
SendText("should resume- NAVIGATING_TAG_SPOTTED");
Transition(RobotState::NAVIGATING_TAG_FINISHED);
}
else
{
ROS_INFO_STREAM_THROTTLE(1, "Waiting on the OK to resume from the tag processor");
}
if (ros::Time::now() - m_timeEnteringState > ros::Duration(5))
{
ROS_ERROR_STREAM("Could not find any tags while stopped. we are going to just resume NAVIGATING");
Transition(RobotState::NAVIGATING);
m_tagProcessor->SetShouldPause(false);
}
break;
case RobotState::NAVIGATING_TAG_FINISHED:
if (ros::Time::now() - m_timeEnteringState > ros::Duration(2))
{
SendSound("mario_pause.wav");
SendText("resuming - NAVIGATING_TAG_FINISHED");
ROS_INFO_STREAM("Transitioning back to navigating");
Transition(RobotState::NAVIGATING);
}
break;
case RobotState::DUMPING:
{
// Check if reached dump site yet
actionlib::SimpleClientGoalState::StateEnum result = action_client_ptr->getState().state_;
switch (result)
{
case actionlib::SimpleClientGoalState::SUCCEEDED:
ROS_INFO_STREAM("Movebase reached target.");
SendDumpFinished(TaskResult::SUCCESS);
Transition(RobotState::DUMPING_FINISHED);
break;
case actionlib::SimpleClientGoalState::ABORTED:
case actionlib::SimpleClientGoalState::REJECTED:
case actionlib::SimpleClientGoalState::LOST:
case actionlib::SimpleClientGoalState::RECALLED:
case actionlib::SimpleClientGoalState::PREEMPTED:
ROS_ERROR_STREAM("Navigation Failed: " << action_client_ptr->getState().toString() );
SendDumpFinished(TaskResult::FAILURE);
Transition(RobotState::WAITING);
break;
case actionlib::SimpleClientGoalState::ACTIVE:
case actionlib::SimpleClientGoalState::PENDING:
default:
ROS_INFO_STREAM_THROTTLE(10, "Navigation state = " << action_client_ptr->getState().toString() );
break;
}
break;
}
case RobotState::DUMPING_FINISHED:
break;
default:
ROS_ERROR_STREAM_THROTTLE(5, "Unexpected State:" << RobotState::ToString(m_status.GetState()) );
break;
}
}
// Convert current cartesian commands to joint velocity
void computeJointVelocity(Eigen::VectorXd& jvel) {
if(jvel.size() != numdof) {
jvel.resize(numdof);
}
if(est_ee_ft.norm() > max_ee_ft_norm) {
for(int i=0; i<jvel.size(); ++i) {
jvel[i]=0.0;
}
ROS_WARN_THROTTLE(0.1, "Too much force! Sending zero velocity");
return;
}
// tf::Pose curr_ee_pose;
mRobot->setJoints(read_jpos);
mRobot->getEEPose(curr_ee_pose);
// Two kinds of cartesian velocities - due to position error and force error
Eigen::VectorXd vel_due_to_pos, vel_due_to_force;
if(bOrientCtrl) {
vel_due_to_pos.resize(6);
vel_due_to_force.resize(6);
} else {
vel_due_to_pos.resize(3);
vel_due_to_force.resize(3);
}
for(int i=0; i<vel_due_to_force.size(); ++i) {
vel_due_to_force[i] = 0;
vel_due_to_pos[i] = 0;
}
ROS_INFO_STREAM_THROTTLE(0.5, "Publishing EE TF");
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(des_ee_pose, ros::Time::now(), world_frame, "/des_ee_tf"));
br.sendTransform(tf::StampedTransform(curr_ee_pose, ros::Time::now(), world_frame, "/curr_ee_tf"));
// Cartesian velocity due to position/orientation error
tf::Vector3 linvel = des_ee_pose.getOrigin() - curr_ee_pose.getOrigin();
vel_due_to_pos(0) = linvel.getX();
vel_due_to_pos(1) = linvel.getY();
vel_due_to_pos(2) = linvel.getZ();
double pos_err = linvel.length();
ROS_INFO_STREAM_THROTTLE(0.5, "Position Err:\t"<< pos_err);
// Nadia's way...
// If Orientation is activated from launch file
double qdiff = acos(abs(des_ee_pose.getRotation().dot(curr_ee_pose.getRotation())));
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<qdiff);
}
if (pos_err < 0.01 && qdiff < 0.05){ // LASA 0.5deg
// if (pos_err < 0.015 && qdiff < 0.05){ // Boxy 1.7 deg
vel_due_to_pos(3) = 0;
vel_due_to_pos(4) = 0;
vel_due_to_pos(5) = 0;
}
/*
// Aswhini's way...
// If Orientation is activated from launch file
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
tf::Quaternion t = angvel;
// ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<angvel.length());
}
*/
// Compute errors in position
double err = vel_due_to_pos.norm();
// Increase tracking performance by tuning traj_traking_gain
vel_due_to_pos *= traj_tracking_gain;
// If force is activated from launch file
if(bUseForce) {
// Compute force error
Eigen::VectorXd ft_err = des_ee_ft - est_ee_ft;
double ft_err_nrm;
if(bOrientCtrl) {
ft_err_nrm = ft_err.norm();
} else {
double force_error = ft_err[0]*ft_err[0] +ft_err[1]*ft_err[1]+ft_err[2]*ft_err[2];
ft_err_nrm = sqrt(force_error);
}
// Remove force if too small. This is necessary due to residual errors in EE force estimation.
// Dont apply external forces until desired position is almost reached within reach_tol
if(ft_err_nrm > ft_tol && err < reach_tol) {
forceErrorToVelocity(ft_err, vel_due_to_force);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force Err:\t"<<ft_err_nrm);
}
// Add the cartesian velocities due to position and force errors and compute the joint_velocity
// mRobot->getIKJointVelocity(vel_due_to_force+vel_due_to_pos, jvel);
mRobot->getIKJointVelocity(vel_due_to_pos, jvel);
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example2" );
ros::NodeHandle n;
const double val = 3.14;
// Basic messages:
ROS_INFO( "My INFO message." );
ROS_INFO( "My INFO message with argument: %f", val );
ROS_INFO_STREAM(
"My INFO stream message with argument: " << val
);
// Named messages:
ROS_INFO_STREAM_NAMED(
"named_msg",
"My named INFO stream message; val = " << val
);
// Conditional messages:
ROS_INFO_STREAM_COND(
val < 0.,
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND(
val >= 0.,
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Conditional Named messages:
ROS_INFO_STREAM_COND_NAMED(
val < 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND_NAMED(
val >= 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Filtered messages:
struct MyLowerFilter : public ros::console::FilterBase {
MyLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;
}
double value;
};
struct MyGreaterEqualFilter : public ros::console::FilterBase {
MyGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.;
}
double value;
};
MyLowerFilter filter_lower( val );
MyGreaterEqualFilter filter_greater_equal( val );
ROS_INFO_STREAM_FILTER(
&filter_lower,
"My filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"My filter INFO stream message; val (" << val << ") >= 0"
);
// Once messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_ONCE(
"My once INFO stream message; i = " << i
);
}
// Throttle messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_THROTTLE(
2,
"My throttle INFO stream message; i = " << i
);
ros::Duration( 1 ).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
