// 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; }