ScitosBase.cpp
Go to the documentation of this file.
00001 #include "ScitosBase.h"
00002 
00003 
00004 //      FEATURE_BOOL = 0,  /**< A boolean feature. */
00005 //      FEATURE_INT,       /**< A integer feature. */
00006 //      FEATURE_FLOAT,     /**< A float feature. */
00007 //      FEATURE_STRING     /**< A string feature. */
00008 
00009 template<>
00010 void ScitosBase::setFeature<bool>(std::string name, bool value) {
00011         robot_->setFeatureBool(name, value);
00012 }
00013 template<>
00014 void ScitosBase::setFeature<int>(std::string name, int value) {
00015         robot_->setFeatureInt(name, value);
00016 }
00017 template<>
00018 void ScitosBase::setFeature<float>(std::string name, float value) {
00019         robot_->setFeatureFloat(name, value);
00020 }
00021 template<>
00022 void ScitosBase::setFeature<std::string>(std::string name, std::string value) {
00023         robot_->setFeatureString(name, value);
00024 }
00025 
00026 
00027 template<>
00028 bool ScitosBase::getFeature(std::string name) {
00029         bool value;
00030         robot_->getFeatureBool(name, value);
00031         return value;
00032 }
00033 template<>
00034 int ScitosBase::getFeature(std::string name) {
00035         int value;
00036         robot_->getFeatureInt(name, value);
00037         return value;
00038 }
00039 template<>
00040 float ScitosBase::getFeature(std::string name) {
00041         float value;
00042         robot_->getFeatureFloat(name, value);
00043         return value;
00044 }
00045 template<>
00046 std::string ScitosBase::getFeature(std::string name) {
00047         MString value;
00048         robot_->getFeatureString(name, value);
00049         return value;
00050 }
00051 
00052 
00053 
00054 ScitosBase::ScitosBase(const char* config_file, int pArgc, char* pArgv[], ros::NodeHandle& nh) :
00055         node_handle_(nh),
00056         dynamic_reconfigure_server_(dynamic_reconfigure_mutex_),
00057         sonar_is_requested_(false)
00058 {
00059         using namespace MetraLabs::base;
00060         using namespace MetraLabs::robotic::base;
00061         using namespace MetraLabs::robotic::robot;
00062 
00063         Error tErr;
00064 
00066         // Initialization
00067 
00068         // Create a general application object. This will also initialize
00069         // the library MetraLabsBase with the command line arguments.
00070         app_ = new Application(pArgc, pArgv);
00071         if (app_ == NULL) {
00072                 ROS_FATAL("Can't create the application!");
00073                 exit(-1);
00074         }
00075 
00076         // Get the class factory from the application
00077         class_factory_ = app_->getClassFactory();
00078         if (class_factory_ == NULL) {
00079                 ROS_FATAL("Cannot get the ClassFactory!");
00080                 exit(-1);
00081         }
00082 
00083         // Load some parameters for the robot SCITOS-G5
00084         ParameterNode tRobotCfg("RobotCfg");
00085         if ((tErr = tRobotCfg.readFromFile(config_file)) != OK) {
00086                 ROS_FATAL("Can't read parameter file. Code: %s", getErrorString(tErr).c_str());
00087                 exit(-1);
00088         }
00089 
00091 
00092         // Get the blackboard
00093         blackboard_ = app_->getBlackboard();
00094         if (blackboard_ == NULL) {
00095                 ROS_FATAL("Cannot get the Blackboard!");
00096                 exit(-1);
00097         }
00098 
00100         // Robot creation
00101 
00102         // Create the robot interface for SCITOS-G5
00103         robot_ = createInstance<Robot>(class_factory_,
00104                         "b07fb034-83c1-446c-b2df-0dd6aa46eef6");
00105         if (robot_ == NULL) {
00106                 ROS_FATAL("Failed to create the robot.");
00107                 exit(-1);
00108         }
00109 
00110         // Pre-Initialize the robot
00111         int tries = 3;
00112         while(tries-->0 && (tErr = robot_->preInitializeClient(&tRobotCfg)) != OK) {
00113                 ROS_WARN("Failed to pre-initialize the robot. Waiting and retrying..");
00114                 sleep(2);
00115         }
00116         if (tErr != OK) {
00117                 ROS_FATAL("Failed to pre-initialize the robot. Code: %s", getErrorString(tErr).c_str());
00118                 exit(-1);
00119         }
00120 
00121         // Assign robot to blackboard
00122         robot_->setPhysicalName("Robot",                  "MyRobot");
00123         robot_->setPhysicalName("BatteryState",           "MyRobot.BatteryState");
00124         robot_->setPhysicalName("Drive.Odometry",         "MyRobot.Odometry");
00125         robot_->setPhysicalName("Drive.VelocityCmd",      "MyRobot.VelocityCmd");
00126         robot_->setPhysicalName("RangeFinder.Sonar.Data", "MyRobot.Sonar");
00127         robot_->setPhysicalName("Bumper.Bumper.Data",     "MyRobot.Bumper");
00128         robot_->setPhysicalName("Bumper.Bumper.ResetCmd", "MyRobot.BumperResetCmd");
00129         if ((tErr = robot_->assignToBlackboard(blackboard_, true)) != OK) {
00130                 ROS_FATAL("Failed to assign the robot to the blackboard. Code: %s", getErrorString(tErr).c_str());
00131                 exit(-1);
00132         }
00133 
00134         // Initialize the robot
00135         if ((tErr = robot_->initializeClient(&tRobotCfg)) != OK) {
00136                 ROS_FATAL("Failed to initialize the robot. Code: %s", getErrorString(tErr).c_str());
00137                 exit(-1);
00138         }
00139 
00141         // Blackboard data: register listeners
00142 
00143         // Odometry
00144         odometry_data_ = NULL;
00145         tErr = getDataFromBlackboard<BlackboardDataOdometry>(blackboard_,
00146                         "MyRobot.Odometry", odometry_data_);
00147         if (tErr != OK) {
00148                 ROS_FATAL("Failed to get the odometry data from the blackboard. Code: %s", getErrorString(tErr).c_str());
00149                 exit(-1);
00150         }
00151         odometry_data_->addCallback(this);
00152 
00153         // Sonar
00154         sonar_data_ = NULL;
00155         tErr = getDataFromBlackboard<BlackboardDataRange>(blackboard_,
00156                         "MyRobot.Sonar", sonar_data_);
00157         if (tErr != OK) {
00158                 ROS_FATAL("Failed to get the Sonar data from the blackboard. Is it specified in the XML config? Code: %s", getErrorString(tErr).c_str());
00159 //              exit(-1);  no, let the robot start wihtout sonar, even if it wasn't specified in the XML config.
00160         } else {
00161                 sonar_data_->addCallback(this);
00162         }
00163 
00164         // Battery
00165         battery_state_data_ = NULL;
00166         tErr = getDataFromBlackboard<BlackboardDataBatteryState>(blackboard_,
00167                         "MyRobot.BatteryState", battery_state_data_);
00168         if (tErr != OK) {
00169                 ROS_FATAL("Failed to get the battery state data from the blackboard. Code: %s", getErrorString(tErr).c_str());
00170                 exit(-1);
00171         }
00172         // battery_state_data_->addCallback(this);  // replaced with diagnostics thread fetching on demand
00173 
00174         // Bumper
00175         bumper_data_ = NULL;
00176         tErr = getDataFromBlackboard<BlackboardDataBumper>(blackboard_,
00177                         "MyRobot.Bumper", bumper_data_);
00178         if (tErr != OK) {
00179                 ROS_FATAL("Failed to get the bumper data from the blackboard. Code: %s", getErrorString(tErr).c_str());
00180                 exit(-1);
00181         }
00182         bumper_data_->addCallback(this);
00183 
00185         // Blackboard data: register publishers
00186 
00187         // Bumper reset command
00188         bumper_reset_cmd_ = NULL;
00189         tErr = getDataFromBlackboard<BlackboardDataUInt8>(blackboard_,
00190                         "MyRobot.BumperResetCmd", bumper_reset_cmd_);
00191         if (tErr != OK) {
00192                 ROS_FATAL("Failed to get the bumper reset command from the blackboard. Code: %s", getErrorString(tErr).c_str());
00193                 exit(-1);
00194         }
00195 
00196         // Velocity command
00197         velocity_cmd_ = NULL;
00198         tErr = getDataFromBlackboard<BlackboardDataVelocity>(blackboard_,
00199                         "MyRobot.VelocityCmd", velocity_cmd_);
00200         if (tErr != OK) {
00201                 ROS_FATAL("Failed to get the velocity command from the blackboard. Code: %s", getErrorString(tErr).c_str());
00202                 exit(-1);
00203         }
00204 
00205 
00207         // Activate ROS interface
00208 
00209         odom_publisher_ = node_handle_.advertise<nav_msgs::Odometry>("/odom", 20);
00210         sonar_publisher_ = node_handle_.advertise<sensor_msgs::Range>("/sonar", 50);
00211         bumper_publisher_ = node_handle_.advertise<metralabs_msgs::ScitosG5Bumper>("/bumper", 20);
00212 
00213         cmd_vel_subscriber_ = node_handle_.subscribe("/cmd_vel", 1, &ScitosBase::driveCommandCallback, this);
00214         bumper_reset_subscriber_ = node_handle_.subscribe("/bumper_reset", 1, &ScitosBase::bumperResetCallback, this);
00215 
00216         // init automatic sonar de/activatior
00217         boost::thread(&ScitosBase::checkSubscribersLoop, this, ros::Rate(1));
00218 
00219 
00221         // Activation
00222 
00223         // Start the blackboard
00224         if ((tErr = blackboard_->startBlackboard()) != OK) {
00225                 ROS_FATAL("Failed to start the blackboard. Code: %s", getErrorString(tErr).c_str());
00226                 exit(-1);
00227         }
00228 
00230         // Start the robot
00231         if ((tErr = robot_->startClient()) != OK) {
00232                 ROS_FATAL("Failed to start the robot system. Code: %s", getErrorString(tErr).c_str());
00233                 exit(-1);
00234         }
00235 
00236 
00238 
00239         ROS_INFO("Starting diagnostics...");
00240 
00241         diagnostics_publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 50);
00242         boost::thread(&ScitosBase::diagnosticsPublishingLoop, this, ros::Rate(2));
00243 
00244 
00246 
00247         ROS_INFO("Starting dynamic_reconfigure...");
00248 
00249         // update config to current hardware state
00250         metralabs_ros::ScitosG5Config config;
00251         getFeatures(config);    // The first time the read config is totally wrong, maybe it's the previous state from rosparam
00252 //      ROS_INFO_STREAM("This is what the first read gives... " << config.EBC1_Enable24V);
00253 //      getFeatures(config);    // It's no timing issue as far as I tested it.          TODO fix or proof as stable..
00254 
00255         // mutex needed for dynreconf.updateConfig, see non existent manual, eh I mean source
00256         boost::recursive_mutex::scoped_lock dyn_reconf_lock(dynamic_reconfigure_mutex_);
00257 //      ROS_INFO_STREAM("Updating with current config from hardware... " << config.EBC1_Enable24V);
00258         dynamic_reconfigure_server_.updateConfig(config);
00259         dyn_reconf_lock.unlock();
00260 
00261         // init reconfigure publisher
00262         boost::thread(&ScitosBase::dynamicReconfigureUpdaterLoop, this, boost::ref(dynamic_reconfigure_server_),
00263                         boost::ref(dynamic_reconfigure_mutex_), ros::Rate(2));
00264 
00265         // init reconfigure callback
00266         dynamic_reconfigure::Server<metralabs_ros::ScitosG5Config>::CallbackType dyn_reconf_callback_ptr;
00267         dyn_reconf_callback_ptr = boost::bind(&ScitosBase::dynamicReconfigureCallback, this, _1, _2);
00268         dynamic_reconfigure_server_.setCallback(dyn_reconf_callback_ptr);
00269 
00270 }
00271 
00272 ScitosBase::~ScitosBase() {
00273         setFeature(FEATURE_SONAR, false);
00274 
00275         Error tErr;
00276         if ((tErr = blackboard_->stopBlackboard()) != OK)
00277                 ROS_ERROR("Failed to stop the blackboard. Code: %s", getErrorString(tErr).c_str());
00278 
00279         // Stop the robot.
00280         if ((tErr = robot_->stopClient()) != OK)
00281                 ROS_ERROR("Failed to stop the robot system. Code: %s", getErrorString(tErr).c_str());
00282 
00283         // Destroy the robot
00284         if ((tErr = robot_->destroyClient()) != OK)
00285                 ROS_ERROR("Failed to destroy the robot system. Code: %s", getErrorString(tErr).c_str());
00286 
00287         // Delete the application object
00288         delete app_;
00289 }
00290 
00291 
00292 void ScitosBase::dynamicReconfigureCallback(metralabs_ros::ScitosG5Config& config, uint32_t level) {
00293         // I wrote this macro because I couldn't find a way to read the configs parameters generically,
00294         // and with this macro the actual feature name only has to be named once. Improvements welcome.
00295         #define MAKRO_SET_FEATURE(NAME) \
00296                 ROS_INFO("Setting feature %s to %s", #NAME, config.NAME?"True":"False"); \
00297                 setFeature(#NAME, config.NAME)
00298 
00299         MAKRO_SET_FEATURE(EBC0_Enable5V);
00300         MAKRO_SET_FEATURE(EBC0_Enable12V);
00301         MAKRO_SET_FEATURE(EBC0_Enable24V);
00302         MAKRO_SET_FEATURE(EBC1_Enable5V);
00303         MAKRO_SET_FEATURE(EBC1_Enable12V);
00304         MAKRO_SET_FEATURE(EBC1_Enable24V);
00305         MAKRO_SET_FEATURE(FreeRunMode);
00306         MAKRO_SET_FEATURE(SonarsActive);
00307         MAKRO_SET_FEATURE(StatusDisplayKnobLock);
00308         MAKRO_SET_FEATURE(StatusDisplayLED);
00309 
00310         ROS_DEBUG("Now reading again: (why is this rubbish?)"); // TODO fix me
00311         metralabs_ros::ScitosG5Config config_read;
00312         getFeatures(config_read);
00313 }
00314 
00315 void ScitosBase::getFeatures(metralabs_ros::ScitosG5Config& config) {
00316         // I wrote this macro because I couldn't find a way to read the configs parameters generically,
00317         // and with this macro the actual feature name only has to be named once. Improvements welcome.
00318         #define MAKRO_GET_FEATURE(NAME) \
00319                 ROS_DEBUG("Current hardware feature %s is %s", #NAME, config.NAME?"True":"False"); \
00320                 config.NAME = getFeature<typeof(config.NAME)>(std::string(#NAME))
00321 
00322         MAKRO_GET_FEATURE(EBC0_Enable5V);
00323         MAKRO_GET_FEATURE(EBC0_Enable12V);
00324         MAKRO_GET_FEATURE(EBC0_Enable24V);
00325         MAKRO_GET_FEATURE(EBC1_Enable5V);
00326         MAKRO_GET_FEATURE(EBC1_Enable12V);
00327         MAKRO_GET_FEATURE(EBC1_Enable24V);
00328         MAKRO_GET_FEATURE(FreeRunMode);
00329         MAKRO_GET_FEATURE(SonarsActive);
00330         MAKRO_GET_FEATURE(StatusDisplayKnobLock);
00331         MAKRO_GET_FEATURE(StatusDisplayLED);
00332 }
00333 
00334 
00335 void ScitosBase::odometryCallbackHandler() {
00336         MTime time;
00337         Pose pose;
00338         Velocity velocity;
00339 
00340         odometry_data_->readLock();
00341         odometry_data_->getData(pose, velocity);
00342         time = odometry_data_->getTimeStamp();
00343         odometry_data_->readUnlock();
00344 
00346         ros::Time odom_time = ros::Time().fromNSec(time.getTimeValue()*1000000);
00347         double x = pose.getX();
00348         double y = pose.getY();
00349         double th = pose.getPhi();
00350         double vx = velocity.getVelocityTranslational();
00351         double vth = velocity.getVelocityRotational();
00352 
00353         // since all odometry is 6DOF we'll need a quaternion created from yaw
00354         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
00355 
00357         geometry_msgs::TransformStamped odom_tf;
00358         odom_tf.header.stamp = odom_time;
00359         odom_tf.header.frame_id = "/odom";
00360         odom_tf.child_frame_id = "/base_link";
00361 
00362         odom_tf.transform.translation.x = x;
00363         odom_tf.transform.translation.y = y;
00364         odom_tf.transform.rotation = odom_quat;
00365 
00366         // publish the transform
00367         tf_broadcaster_.sendTransform(odom_tf);
00368 
00370         nav_msgs::Odometry odom_msg;
00371         odom_msg.header.stamp = odom_time;
00372         odom_msg.header.frame_id = "/odom";
00373         odom_msg.child_frame_id = "/base_link";
00374 
00375         // set the position
00376         odom_msg.pose.pose.position.x = x;
00377         odom_msg.pose.pose.position.y = y;
00378         odom_msg.pose.pose.orientation = odom_quat;
00379 
00380         // set the velocity
00381         odom_msg.twist.twist.linear.x = vx;
00382         odom_msg.twist.twist.angular.z = vth;
00383 
00384         // publish the message
00385         odom_publisher_.publish(odom_msg);
00386 }
00387 
00388 void ScitosBase::sonarCallbackHandler() {
00389         static const RangeData::Config* sonar_config = NULL;
00390 
00391         sonar_data_->readLock();
00392         MTime timestamp = odometry_data_->getTimeStamp();
00393         const RangeData::Vector& sonar_data = sonar_data_->getRangeData();
00394         if (sonar_config == NULL)
00395                 sonar_config = sonar_data_->getConfig();
00396         sonar_data_->readUnlock();
00397 
00398         const std::vector<RangeData::Measurement> measurements = sonar_data;
00399         ros::Time sonar_time = ros::Time().fromNSec(timestamp.getTimeValue()*1000000);
00400 
00401 
00402         if(!sonar_is_requested_) {
00403                 setFeature(FEATURE_SONAR, false);
00404         } else {
00405                 if (sonar_config == NULL) {
00406                         ROS_WARN_THROTTLE(0.5, "Could not yet read sonar config.");
00407                 } else {
00408                         // if config is loaded, proceed..
00409                         const RangeData::ConfigCircularArc* sonar_config_circular =
00410                                         dynamic_cast<const RangeData::ConfigCircularArc*>(sonar_config);
00411 
00413 
00414                         // calculate transforms (once)
00415                         static std::vector<tf::Transform> sonar_transforms;
00416                         static bool sonar_transforms_calculated = false;
00417 
00418                         if(!sonar_transforms_calculated) {
00419                                 sonar_transforms_calculated = true;
00420                                 float angle = sonar_config_circular->first_sensor_angle;
00421                                 for (unsigned int i = 0; i < sonar_config_circular->sensor_cnt; ++i) {
00422                                         float x = sonar_config_circular->offset_from_center * std::cos(angle) -0.075;
00423                                         float y = sonar_config_circular->offset_from_center * std::sin(angle);
00424                                         tf::Quaternion quat;
00425                                         quat.setEuler(0, 0, angle);
00426                                         tf::Transform* transform = new tf::Transform(quat, tf::Vector3(x, y, 0.25));
00427                                         sonar_transforms.push_back(*transform);
00428                                         angle += sonar_config_circular->sensor_angle_dist;
00429                                 }
00430                                 // broadcast all transforms once
00431                                 std::vector<tf::Transform>::iterator it = sonar_transforms.begin();
00432                                 for (int i = 0; it != sonar_transforms.end(); ++it) {
00433                                         char targetframe[20];
00434                                         sprintf(targetframe, "/sonar/sonar_%02d", i++);
00435                                         tf_broadcaster_.sendTransform(
00436                                                         tf::StampedTransform(*it, sonar_time, "/base_link", targetframe)
00437                                                 );
00438                                 }
00439                         }
00440 
00442 
00443                         sensor_msgs::Range sonar_msg;
00444                         sonar_msg.header.stamp = sonar_time;
00445                         sonar_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
00446                         sonar_msg.field_of_view = sonar_config_circular->cone_angle;  // DEG_TO_RAD(15);        // from manual
00447                         sonar_msg.min_range = 0.2;      // from manual
00448                         sonar_msg.max_range = 3;        // from manual
00449 
00450                         // each time send only one sonar data and one transform to reduce repeating messages
00451                         {
00452                                 static int next_sonar_to_send = 0;
00453                                 char next_sonar_frame[20];
00454                                 sprintf(next_sonar_frame, "/sonar/sonar_%02d", next_sonar_to_send);
00455 
00456                                 // range data
00457                         //      ROS_DEBUG("Sonar #%d: value: %1.3f status: %d", next_sonar_to_send, measurements.at(next_sonar_to_send).range, measurements.at(next_sonar_to_send).err.std);
00458                                 switch(measurements.at(next_sonar_to_send).err.std) {
00459                                         case RangeData::RANGE_OKAY:
00460                                         case RangeData::RANGE_OBJECT_SOMEWHERE_IN_RANGE:
00461                                         case RangeData::RANGE_PROBABLY_OKAY:
00462                                         case RangeData::RANGE_PROBABLY_OBJECT_SOMEWHERE_IN_RANGE:
00463                                                 sonar_msg.range = measurements.at(next_sonar_to_send).range;
00464                                                 break;
00465                                         case RangeData::RANGE_NO_OBJECT_WITHIN_RANGE:
00466                                         case RangeData::RANGE_PROBABLY_NO_OBJECT_WITHIN_RANGE:
00467                                         case RangeData::RANGE_INVALID_MEASUREMENT:
00468                                         case RangeData::RANGE_ERR_SENSOR_BROKEN:
00469                                         case RangeData::RANGE_MASKED:
00470                                         default:
00471                                                 ;// TODO maybe send a zero-range message to make the old value obsolete, if that isn't useful
00472                                                 sonar_msg.range = 0;
00473                                 }
00474                                 sonar_msg.header.frame_id = next_sonar_frame;
00475                                 sonar_publisher_.publish(sonar_msg);
00476 
00477                                 // resend also one transform (the according, why not)
00478                                 tf_broadcaster_.sendTransform( tf::StampedTransform(
00479                                                 sonar_transforms.at(next_sonar_to_send), ros::Time::now(), "/base_link", next_sonar_frame ) );
00480 
00481                                 ++next_sonar_to_send %= measurements.size();
00482                         }
00483 
00484 //              this is code to send all sonar measurements at one time
00485 //                      std::vector<RangeData::Measurement>::iterator itM = measurements.begin();
00486 //                      for(int i=0; itM != measurements.end(); ++itM) {
00488 //
00489 //                              char targetframe[20];
00490 //                              sprintf(targetframe, "/sonar/sonar_%2d", i++);
00491 //                              sonar.header.frame_id = targetframe;
00492 //                              sonar.range = itM->range+1;
00493 //
00494 //                              //publish the message
00495 //                              sonar_publisher_.publish(sonar);
00496 //                      }
00497 
00498                 } // if sonar config loaded
00499         } // if sonar active
00500 }
00501 
00502 void ScitosBase::bumperDataCallbackHandler() {
00503         bumper_data_->readLock();
00504         BumperData::Vector bumper_values = bumper_data_->getBumperData();
00505         MTime timestamp = odometry_data_->getTimeStamp();
00506         bumper_data_->readUnlock();
00507 
00508 #define BUMPER_CODE_FLAGS_EMPTY   0x00
00509 #define BUMPER_CODE_FLAG_NORMAL                                         (1<<0)
00510 #define BUMPER_CODE_FLAG_MOTOR_STOPPED                          (1<<1)
00511 #define BUMPER_CODE_FLAG_FREE_RUN_ACTIVE                        (1<<2)
00512 #define BUMPER_CODE_FLAG_EMERGENCY_BUTTON_PRESSED       (1<<3)
00513 #define BUMPER_CODE_FLAG_BUMPER_PRESSED                         (1<<4)
00514 #define BUMPER_CODE_FLAG_BUS_ERROR                                      (1<<5)
00515 #define BUMPER_CODE_FLAG_STALL_MODE_DETECTED            (1<<6)
00516 #define BUMPER_CODE_FLAG_INTERNAL_ERROR                         (1<<7)
00517 
00518         // map all bumper parts' flags together
00519         uint16_t flags = 0;
00520         for (BumperData::Vector::const_iterator it = bumper_values.begin(); it != bumper_values.end(); ++it)
00521                 flags |= *it;
00522 
00523         bool bumper_pressed = flags & BUMPER_CODE_FLAG_BUMPER_PRESSED;
00524         bool motor_stop = flags & BUMPER_CODE_FLAG_MOTOR_STOPPED;
00525 
00526         if (bumper_pressed)                     // remove me!
00527                 // make sure code and people can rely on bumper_pressed
00528                 ROS_ASSERT_MSG(motor_stop,
00529                                 "Bumper: MOTOR_STOP is not set although BUMPER_PRESSED is set");
00530 
00531         metralabs_msgs::ScitosG5Bumper bumper_msg;
00532         bumper_msg.header.stamp = ros::Time().fromNSec(timestamp.getTimeValue()*1000000);
00533         bumper_msg.bumper_pressed = bumper_pressed;
00534         bumper_msg.motor_stop = motor_stop;
00535 
00536         bumper_publisher_.publish(bumper_msg);
00537 }
00538 
00539 void ScitosBase::diagnosticsPublishingLoop(ros::Rate loop_rate) {
00540         std::stringstream ss; // policy: reset ss before using, but clear formatting afterwards
00541         ss.setf(ss.fixed);
00542 
00543         // send diagnostics message for battery state but with lower frequency
00544         // robot itself publishes with 10 Hz into Blackboard
00545         while (node_handle_.ok()) {
00546                 diagnostic_msgs::DiagnosticArray diag_array;
00547                 // only one timestamp for a DiagArray, not for each status
00548                 // but we won't send statii twice
00549                 diag_array.header.stamp = ros::Time::now();
00550 
00552                 {
00553                         battery_state_data_->readLock();
00554                         MString name = static_cast<BatteryState*>(battery_state_data_)->getName();
00555                         MTime timestamp = battery_state_data_->getTimeStamp();
00556                         float voltage = battery_state_data_->getVoltage();
00557                         float current = battery_state_data_->getCurrent();
00558                         int16_t charge_state = battery_state_data_->getChargeState();
00559                         int16_t remaining_time = battery_state_data_->getRemainingTime();
00560                         int16_t charger_status= battery_state_data_->getChargerStatus();
00561                         battery_state_data_->readUnlock();
00562 
00563                         static MTime last_timestamp = 0;
00564                         if (timestamp > last_timestamp) {
00565                                 last_timestamp = timestamp;
00566 
00567                                 diagnostic_msgs::DiagnosticStatus battery_status;
00568                                 battery_status.hardware_id = name;
00569                                 battery_status.name = "Scitos G5: Battery";
00570                                 battery_status.message = "undefined";
00571 
00572 // TODO do me parameters
00573 #define         VOLTAGE_ERROR_LEVEL     23              // and below
00574 #define         VOLTAGE_WARN_LEVEL      24              // and below
00575 #define         VOLTAGE_MID_LEVEL       26              // and below // above means HIGH_LEVEL
00576 #define         VOLTAGE_FULL_LEVEL      28.8    // and above
00577 #define         CHARGER_PLUGGED         1
00578 
00579                                 // set level
00580                                 if(voltage < VOLTAGE_ERROR_LEVEL && charger_status != CHARGER_PLUGGED)
00581                                         battery_status.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00582                                 else if(voltage < VOLTAGE_WARN_LEVEL && charger_status != CHARGER_PLUGGED)
00583                                         battery_status.level = diagnostic_msgs::DiagnosticStatus::WARN;
00584                                 else
00585                                         battery_status.level = diagnostic_msgs::DiagnosticStatus::OK;
00586 
00587                                 // set text message
00588                                 if(voltage >= VOLTAGE_FULL_LEVEL)
00589                                         battery_status.message = "Fully charged";
00590                                 else {
00591                                         battery_status.message = "High";
00592                                         if(voltage < VOLTAGE_MID_LEVEL)
00593                                                 battery_status.message = "Mid";
00594                                         if(voltage < VOLTAGE_WARN_LEVEL)
00595                                                 battery_status.message = "Low";
00596                                         if(voltage < VOLTAGE_ERROR_LEVEL)
00597                                                 battery_status.message = "Depleted";
00598                                         battery_status.message += charger_status == CHARGER_PLUGGED ? ", charging" : ", discharging";
00599                                 }
00600 
00601                                 // set detail values
00602                                 battery_status.values.resize(5);
00603 
00604                                 ss.str("");
00605                                 int prev_prec = ss.precision(3);
00606                                 battery_status.values[0].key = "Voltage";
00607                                 ss << voltage << " V";
00608                                 battery_status.values[0].value = ss.str();
00609 
00610                                 ss.str("");
00611                                 battery_status.values[1].key = "Current";
00612                                 ss << current << " A";
00613                                 battery_status.values[1].value = ss.str();
00614                                 ss.precision(prev_prec);
00615 
00616                                 ss.str("");
00617                                 battery_status.values[2].key = "ChargeState";
00618                                 ss << charge_state << " %";
00619                                 battery_status.values[2].value = ss.str();
00620 
00621                                 ss.str("");
00622                                 battery_status.values[3].key = "RemainingTime";
00623                                 ss << remaining_time << " min";
00624                                 battery_status.values[3].value = ss.str();
00625 
00626                                 battery_status.values[4].key = "ChargerStatus";
00627                                 battery_status.values[4].value = charger_status == CHARGER_PLUGGED ? "plugged" : "unplugged";
00628 
00629                                 diag_array.status.push_back(battery_status);
00630                         }
00631                 }
00632 
00634                 {
00635                         bumper_data_->readLock();
00636                         MString name = static_cast<BumperData*>(bumper_data_)->getName();
00637                         BumperData::Vector bumper_values = bumper_data_->getBumperData();
00638                         MTime timestamp = odometry_data_->getTimeStamp();
00639                         bumper_data_->readUnlock();
00640 
00641                         static MTime last_timestamp = 0;
00642                         if (timestamp > last_timestamp) {
00643                                 last_timestamp = timestamp;
00644 
00645                                 diagnostic_msgs::DiagnosticStatus bumper_status;
00646                                 bumper_status.hardware_id = name;
00647                                 bumper_status.name = "Scitos G5: Bumper";
00648                                 bumper_status.message = "undefined";
00649 
00650                                 // map all bumper parts' flags together
00651                                 uint16_t flags = 0;
00652                                 for (BumperData::Vector::const_iterator it = bumper_values.begin(); it != bumper_values.end(); ++it)
00653                                         flags |= *it;
00654 
00655                                 // set message and values
00656                                 // note that these cases are primarily ordered with escalating status
00657                                 // level and secondarily with message importance, because they override
00658                                 // each other and the most important should be in the message
00659                                 ss.str("");
00660                                 if (flags == BUMPER_CODE_FLAGS_EMPTY || flags & BUMPER_CODE_FLAG_NORMAL) {
00661                                         ss << "Normal ";
00662                                         bumper_status.message = "Normal";
00663                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::OK;
00664                                 }
00665                                 if (flags & BUMPER_CODE_FLAG_FREE_RUN_ACTIVE) {
00666                                         ss << "FreeRunActive ";
00667                                         bumper_status.message = "FreeRunActive";
00668                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::OK;
00669                                 }
00670                                 if (flags & BUMPER_CODE_FLAG_MOTOR_STOPPED) {
00671                                         ss << "MotorStop ";
00672                                         bumper_status.message = "MotorStop";
00673                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::WARN;
00674                                 }
00675                                 if (flags & BUMPER_CODE_FLAG_BUMPER_PRESSED) {
00676                                         ss << "BumperPressed ";
00677                                         bumper_status.message = "BumperPressed";
00678                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::WARN;
00679                                 }
00680                                 if (flags & BUMPER_CODE_FLAG_EMERGENCY_BUTTON_PRESSED) {
00681                                         ss << "EmergencyButtonPressed ";
00682                                         bumper_status.message = "EmergencyButtonPressed";
00683                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::WARN;
00684                                 }
00685                                 if (flags & BUMPER_CODE_FLAG_STALL_MODE_DETECTED) {
00686                                         ss << "StallModeDetected ";
00687                                         bumper_status.message = "StallModeDetected";
00688                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00689                                 }
00690                                 if (flags & BUMPER_CODE_FLAG_BUS_ERROR) {
00691                                         ss << "BusError ";
00692                                         bumper_status.message = "BusError";
00693                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00694                                 }
00695                                 if (flags & BUMPER_CODE_FLAG_INTERNAL_ERROR) {
00696                                         ss << "InternalError ";
00697                                         bumper_status.message = "InternalError";
00698                                         bumper_status.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00699                                 }
00700                                 diagnostic_msgs::KeyValue diag_bumper_flags;
00701                                 diag_bumper_flags.key = "Active flags";
00702                                 diag_bumper_flags.value = ss.str();
00703                                 bumper_status.values.push_back(diag_bumper_flags);
00704 
00705                                 diag_array.status.push_back(bumper_status);
00706                         }
00707                 }
00708 
00710                 {
00711                         odometry_data_->readLock();
00712                         MString name = static_cast<OdometryData*>(odometry_data_)->getName();
00713                         MTime timestamp = odometry_data_->getTimeStamp();
00714                         float mileage = odometry_data_->getMileage();
00715                         odometry_data_->readUnlock();
00716 
00717                         static MTime last_timestamp = 0;
00718                         if (timestamp > last_timestamp) {
00719                                 last_timestamp = timestamp;
00720 
00721                                 diagnostic_msgs::DiagnosticStatus odometry_status;
00722                                 odometry_status.hardware_id = name;
00723                                 odometry_status.name = "Scitos G5: Mileage";
00724 
00725                                 ss.str("");
00726                                 int prev_prec = ss.precision(1);
00727                                 ss << mileage << " m";
00728                                 ss.precision(prev_prec);
00729                                 odometry_status.message = ss.str();
00730 
00731                                 ss.str("");
00732                                 diagnostic_msgs::KeyValue kv_mileage;
00733                                 kv_mileage.key = "Mileage";
00734                                 ss << mileage << " meters";
00735                                 kv_mileage.value = ss.str();
00736 
00737                                 odometry_status.values.push_back(kv_mileage);
00738 
00739                                 diag_array.status.push_back(odometry_status);
00740                         }
00741                 }
00742 
00744                 diagnostics_publisher_.publish(diag_array);
00745 
00746                 // This will adjust as needed per iteration
00747                 loop_rate.sleep();
00748         }
00749 }
00750 
00751 void ScitosBase::checkSubscribersLoop(ros::Rate loop_rate) {
00752         // set state variable and hardware to same state
00753         sonar_is_requested_ = false;
00754         setFeature(FEATURE_SONAR, false);
00755         while (node_handle_.ok()) {
00757                 bool sonar_had_been_requested = sonar_is_requested_;
00758                 sonar_is_requested_ = sonar_publisher_.getNumSubscribers() != 0;
00759                 // this check allows to override sonar state via dynamic reconfigure and avoids overhead
00760                 if(sonar_is_requested_ != sonar_had_been_requested) {
00761                         setFeature(FEATURE_SONAR, sonar_is_requested_);
00762                         ROS_INFO_STREAM("Switching sonar feature to: " << (sonar_is_requested_ ? "enabled" : "disabled"));
00763                 }
00764                 loop_rate.sleep();
00765         }
00766 }
00767 
00768 void ScitosBase::dynamicReconfigureUpdaterLoop(
00769                 dynamic_reconfigure::Server<metralabs_ros::ScitosG5Config> &dynamic_reconfigure_server,
00770                 boost::recursive_mutex &mutex, ros::Rate loop_rate) {
00771         metralabs_ros::ScitosG5Config config;
00772         while (node_handle_.ok()) {
00773                 getFeatures(config);// update config to current hardware state
00774                 boost::recursive_mutex::scoped_lock lock(mutex);
00775                 dynamic_reconfigure_server.updateConfig(config);
00776                 lock.unlock();
00777                 loop_rate.sleep();
00778         }
00779 }


metralabs_ros
Author(s): Yianni Gatsoulis and Chris Burbridge and Lorenzo Riano and Felix Kolbe
autogenerated on Mon Oct 6 2014 07:27:58