slot_callbacks.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Yujin Robot.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Yujin Robot nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00036 /*****************************************************************************
00037 ** Includes
00038 *****************************************************************************/
00039 
00040 #include "kobuki_node/kobuki_ros.hpp"
00041 
00042 /*****************************************************************************
00043  ** Namespaces
00044  *****************************************************************************/
00045 
00046 namespace kobuki
00047 {
00048 
00049 void KobukiRos::processStreamData() {
00050   publishWheelState();
00051   publishSensorState();
00052   publishDockIRData();
00053   publishInertia();
00054   publishRawInertia();
00055 }
00056 
00057 /*****************************************************************************
00058 ** Publish Sensor Stream Workers
00059 *****************************************************************************/
00060 
00061 void KobukiRos::publishSensorState()
00062 {
00063   if ( ros::ok() ) {
00064     if (sensor_state_publisher.getNumSubscribers() > 0) {
00065       kobuki_msgs::SensorState state;
00066       CoreSensors::Data data = kobuki.getCoreSensorData();
00067       state.header.stamp = ros::Time::now();
00068       state.time_stamp = data.time_stamp; // firmware time stamp
00069       state.bumper = data.bumper;
00070       state.wheel_drop = data.wheel_drop;
00071       state.cliff = data.cliff;
00072       state.left_encoder = data.left_encoder;
00073       state.right_encoder = data.right_encoder;
00074       state.left_pwm = data.left_pwm;
00075       state.right_pwm = data.right_pwm;
00076       state.buttons = data.buttons;
00077       state.charger = data.charger;
00078       state.battery = data.battery;
00079       state.over_current = data.over_current;
00080 
00081       Cliff::Data cliff_data = kobuki.getCliffData();
00082       state.bottom = cliff_data.bottom;
00083 
00084       Current::Data current_data = kobuki.getCurrentData();
00085       state.current = current_data.current;
00086 
00087       GpInput::Data gp_input_data = kobuki.getGpInputData();
00088       state.digital_input = gp_input_data.digital_input;
00089       for ( unsigned int i = 0; i < gp_input_data.analog_input.size(); ++i ) {
00090         state.analog_input.push_back(gp_input_data.analog_input[i]);
00091       }
00092 
00093       sensor_state_publisher.publish(state);
00094     }
00095   }
00096 }
00097 
00098 void KobukiRos::publishWheelState()
00099 {
00100   // Take latest encoders and gyro data
00101   ecl::LegacyPose2D<double> pose_update;
00102   ecl::linear_algebra::Vector3d pose_update_rates;
00103   kobuki.updateOdometry(pose_update, pose_update_rates);
00104   kobuki.getWheelJointStates(joint_states.position[0], joint_states.velocity[0],   // left wheel
00105                              joint_states.position[1], joint_states.velocity[1]);  // right wheel
00106 
00107   // Update and publish odometry and joint states
00108   odometry.update(pose_update, pose_update_rates, kobuki.getHeading(), kobuki.getAngularVelocity());
00109 
00110   if (ros::ok())
00111   {
00112     joint_states.header.stamp = ros::Time::now();
00113     joint_state_publisher.publish(joint_states);
00114   }
00115 }
00116 
00117 void KobukiRos::publishInertia()
00118 {
00119   if (ros::ok())
00120   {
00121     if (imu_data_publisher.getNumSubscribers() > 0)
00122     {
00123       // Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
00124       sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
00125 
00126       msg->header.frame_id = "gyro_link";
00127       msg->header.stamp = ros::Time::now();
00128 
00129       msg->orientation = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, kobuki.getHeading());
00130 
00131       // set a non-zero covariance on unused dimensions (pitch and roll); this is a requirement of robot_pose_ekf
00132       // set yaw covariance as very low, to make it dominate over the odometry heading when combined
00133       // 1: fill once, as its always the same;  2: using an invented value; cannot we get a realistic estimation?
00134       msg->orientation_covariance[0] = DBL_MAX;
00135       msg->orientation_covariance[4] = DBL_MAX;
00136       msg->orientation_covariance[8] = 0.05;
00137 
00138       // fill angular velocity; we ignore acceleration for now
00139       msg->angular_velocity.z = kobuki.getAngularVelocity();
00140 
00141       // angular velocity covariance; useless by now, but robot_pose_ekf's
00142       // roadmap claims that it will compute velocities in the future
00143       msg->angular_velocity_covariance[0] = DBL_MAX;
00144       msg->angular_velocity_covariance[4] = DBL_MAX;
00145       msg->angular_velocity_covariance[8] = 0.05;
00146 
00147       imu_data_publisher.publish(msg);
00148     }
00149   }
00150 }
00151 
00152 void KobukiRos::publishRawInertia()
00153 {
00154   if ( ros::ok() && (raw_imu_data_publisher.getNumSubscribers() > 0) )
00155   {
00156     // Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
00157     sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
00158     ThreeAxisGyro::Data data = kobuki.getRawInertiaData();
00159 
00160     ros::Time now = ros::Time::now();
00161     ros::Duration interval(0.01); // Time interval between each sensor reading.
00162     const double digit_to_dps = 0.00875; // digit to deg/s ratio, comes from datasheet of 3d gyro[L3G4200D].
00163     unsigned int length = data.followed_data_length/3;
00164     for( unsigned int i=0; i<length; i++) {
00165       // Each sensor reading has id, that circulate 0 to 255.
00166       //msg->header.frame_id = std::string("gyro_link_" + boost::lexical_cast<std::string>((unsigned int)data.frame_id+i));
00167       msg->header.frame_id = "gyro_link";
00168 
00169       // Update rate of 3d gyro sensor is 100 Hz, but robot's update rate is 50 Hz.
00170       // So, here is some compensation.
00171       // See also https://github.com/yujinrobot/kobuki/issues/216
00172       msg->header.stamp = now - interval * (length-i-1);
00173 
00174       // Sensing axis of 3d gyro is not match with robot. It is rotated 90 degree counterclockwise about z-axis.
00175       msg->angular_velocity.x = angles::from_degrees( -digit_to_dps * (short)data.data[i*3+1] );
00176       msg->angular_velocity.y = angles::from_degrees(  digit_to_dps * (short)data.data[i*3+0] );
00177       msg->angular_velocity.z = angles::from_degrees(  digit_to_dps * (short)data.data[i*3+2] );
00178 
00179       raw_imu_data_publisher.publish(msg);
00180     }
00181   }
00182 }
00183 
00184 void KobukiRos::publishDockIRData()
00185 {
00186   if (ros::ok())
00187   {
00188     if (dock_ir_publisher.getNumSubscribers() > 0)
00189     {
00190       DockIR::Data data = kobuki.getDockIRData();
00191 
00192       // Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
00193       kobuki_msgs::DockInfraRedPtr msg(new kobuki_msgs::DockInfraRed);
00194 
00195       msg->header.frame_id = "dock_ir_link";
00196       msg->header.stamp = ros::Time::now();
00197 
00198       msg->data.push_back( data.docking[0] );
00199       msg->data.push_back( data.docking[1] );
00200       msg->data.push_back( data.docking[2] );
00201 
00202       dock_ir_publisher.publish(msg);
00203     }
00204   }
00205 }
00206 
00207 /*****************************************************************************
00208 ** Non Default Stream Packets
00209 *****************************************************************************/
00216 void KobukiRos::publishVersionInfo(const VersionInfo &version_info)
00217 {
00218   if (ros::ok())
00219   {
00220     kobuki_msgs::VersionInfoPtr msg(new kobuki_msgs::VersionInfo);
00221 
00222     msg->firmware = VersionInfo::toString(version_info.firmware);
00223     msg->hardware = VersionInfo::toString(version_info.hardware);
00224     msg->software = VersionInfo::getSoftwareVersion();
00225 
00226     msg->udid.resize(3);
00227     msg->udid[0] = version_info.udid0;
00228     msg->udid[1] = version_info.udid1;
00229     msg->udid[2] = version_info.udid2;
00230 
00231     // Set available features mask depending on firmware and driver versions
00232     if (version_info.firmware > 65536)  // 1.0.0
00233     {
00234       msg->features |= kobuki_msgs::VersionInfo::SMOOTH_MOVE_START;
00235       msg->features |= kobuki_msgs::VersionInfo::GYROSCOPE_3D_DATA;
00236     }
00237     if (version_info.firmware > 65792)  // 1.1.0
00238     {
00239       // msg->features |= kobuki_msgs::VersionInfo::SOMETHING_JINCHA_FANCY;
00240     }
00241     // if (msg->firmware > ...
00242 
00243     version_info_publisher.publish(msg);
00244   }
00245 }
00246 
00247 void KobukiRos::publishControllerInfo()
00248 {
00249   if (ros::ok())
00250   {
00251     kobuki_msgs::ControllerInfoPtr msg(new kobuki_msgs::ControllerInfo);
00252     ControllerInfo::Data data = kobuki.getControllerInfoData();
00253 
00254     msg->type = data.type;
00255     msg->p_gain = static_cast<float>(data.p_gain) * 0.001f;;
00256     msg->i_gain = static_cast<float>(data.i_gain) * 0.001f;;
00257     msg->d_gain = static_cast<float>(data.d_gain) * 0.001f;;
00258 
00259     controller_info_publisher.publish(msg);
00260   }
00261 }
00262 
00263 /*****************************************************************************
00264 ** Events
00265 *****************************************************************************/
00266 
00267 void KobukiRos::publishButtonEvent(const ButtonEvent &event)
00268 {
00269   if (ros::ok())
00270   {
00271     kobuki_msgs::ButtonEventPtr msg(new kobuki_msgs::ButtonEvent);
00272     switch(event.state) {
00273       case(ButtonEvent::Pressed)  : { msg->state = kobuki_msgs::ButtonEvent::PRESSED;  break; }
00274       case(ButtonEvent::Released) : { msg->state = kobuki_msgs::ButtonEvent::RELEASED; break; }
00275       default: break;
00276     }
00277     switch(event.button) {
00278       case(ButtonEvent::Button0) : { msg->button = kobuki_msgs::ButtonEvent::Button0; break; }
00279       case(ButtonEvent::Button1) : { msg->button = kobuki_msgs::ButtonEvent::Button1; break; }
00280       case(ButtonEvent::Button2) : { msg->button = kobuki_msgs::ButtonEvent::Button2; break; }
00281       default: break;
00282     }
00283     button_event_publisher.publish(msg);
00284   }
00285 }
00286 
00287 void KobukiRos::publishBumperEvent(const BumperEvent &event)
00288 {
00289   if (ros::ok())
00290   {
00291     kobuki_msgs::BumperEventPtr msg(new kobuki_msgs::BumperEvent);
00292     switch(event.state) {
00293       case(BumperEvent::Pressed)  : { msg->state = kobuki_msgs::BumperEvent::PRESSED;  break; }
00294       case(BumperEvent::Released) : { msg->state = kobuki_msgs::BumperEvent::RELEASED; break; }
00295       default: break;
00296     }
00297     switch(event.bumper) {
00298       case(BumperEvent::Left)   : { msg->bumper = kobuki_msgs::BumperEvent::LEFT;   break; }
00299       case(BumperEvent::Center) : { msg->bumper = kobuki_msgs::BumperEvent::CENTER; break; }
00300       case(BumperEvent::Right)  : { msg->bumper = kobuki_msgs::BumperEvent::RIGHT;  break; }
00301       default: break;
00302     }
00303     bumper_event_publisher.publish(msg);
00304   }
00305 }
00306 
00307 void KobukiRos::publishCliffEvent(const CliffEvent &event)
00308 {
00309   if (ros::ok())
00310   {
00311     kobuki_msgs::CliffEventPtr msg(new kobuki_msgs::CliffEvent);
00312     switch(event.state) {
00313       case(CliffEvent::Floor) : { msg->state = kobuki_msgs::CliffEvent::FLOOR; break; }
00314       case(CliffEvent::Cliff) : { msg->state = kobuki_msgs::CliffEvent::CLIFF; break; }
00315       default: break;
00316     }
00317     switch(event.sensor) {
00318       case(CliffEvent::Left)   : { msg->sensor = kobuki_msgs::CliffEvent::LEFT;   break; }
00319       case(CliffEvent::Center) : { msg->sensor = kobuki_msgs::CliffEvent::CENTER; break; }
00320       case(CliffEvent::Right)  : { msg->sensor = kobuki_msgs::CliffEvent::RIGHT;  break; }
00321       default: break;
00322     }
00323     msg->bottom = event.bottom;
00324     cliff_event_publisher.publish(msg);
00325   }
00326 }
00327 
00328 void KobukiRos::publishWheelEvent(const WheelEvent &event)
00329 {
00330   if (ros::ok())
00331   {
00332     kobuki_msgs::WheelDropEventPtr msg(new kobuki_msgs::WheelDropEvent);
00333     switch(event.state) {
00334       case(WheelEvent::Dropped) : { msg->state = kobuki_msgs::WheelDropEvent::DROPPED; break; }
00335       case(WheelEvent::Raised)  : { msg->state = kobuki_msgs::WheelDropEvent::RAISED;  break; }
00336       default: break;
00337     }
00338     switch(event.wheel) {
00339       case(WheelEvent::Left)  : { msg->wheel = kobuki_msgs::WheelDropEvent::LEFT;  break; }
00340       case(WheelEvent::Right) : { msg->wheel = kobuki_msgs::WheelDropEvent::RIGHT; break; }
00341       default: break;
00342     }
00343     wheel_event_publisher.publish(msg);
00344   }
00345 }
00346 
00347 void KobukiRos::publishPowerEvent(const PowerEvent &event)
00348 {
00349   if (ros::ok())
00350   {
00351     kobuki_msgs::PowerSystemEventPtr msg(new kobuki_msgs::PowerSystemEvent);
00352     switch(event.event) {
00353       case(PowerEvent::Unplugged) :
00354         { msg->event = kobuki_msgs::PowerSystemEvent::UNPLUGGED; break; }
00355       case(PowerEvent::PluggedToAdapter) :
00356         { msg->event = kobuki_msgs::PowerSystemEvent::PLUGGED_TO_ADAPTER;  break; }
00357       case(PowerEvent::PluggedToDockbase) :
00358         { msg->event = kobuki_msgs::PowerSystemEvent::PLUGGED_TO_DOCKBASE; break; }
00359       case(PowerEvent::ChargeCompleted)  :
00360         { msg->event = kobuki_msgs::PowerSystemEvent::CHARGE_COMPLETED;  break; }
00361       case(PowerEvent::BatteryLow) :
00362         { msg->event = kobuki_msgs::PowerSystemEvent::BATTERY_LOW; break; }
00363       case(PowerEvent::BatteryCritical) :
00364         { msg->event = kobuki_msgs::PowerSystemEvent::BATTERY_CRITICAL;  break; }
00365       default: break;
00366     }
00367     power_event_publisher.publish(msg);
00368   }
00369 }
00370 
00371 void KobukiRos::publishInputEvent(const InputEvent &event)
00372 {
00373   if (ros::ok())
00374   {
00375     kobuki_msgs::DigitalInputEventPtr msg(new kobuki_msgs::DigitalInputEvent);
00376     for (unsigned int i = 0; i < msg->values.size(); i++)
00377       msg->values[i] = event.values[i];
00378     input_event_publisher.publish(msg);
00379   }
00380 }
00381 
00382 void KobukiRos::publishRobotEvent(const RobotEvent &event)
00383 {
00384   if (ros::ok())
00385   {
00386     kobuki_msgs::RobotStateEventPtr msg(new kobuki_msgs::RobotStateEvent);
00387     switch(event.state) {
00388       case(RobotEvent::Online)  : { msg->state = kobuki_msgs::RobotStateEvent::ONLINE;  break; }
00389       case(RobotEvent::Offline) : { msg->state = kobuki_msgs::RobotStateEvent::OFFLINE; break; }
00390       default: break;
00391     }
00392 
00393     robot_event_publisher.publish(msg);
00394   }
00395 }
00396 
00410 void KobukiRos::publishRawDataCommand(Command::Buffer &buffer)
00411 {
00412   if ( raw_data_command_publisher.getNumSubscribers() > 0 ) { // do not do string processing if there is no-one listening.
00413     std::ostringstream ostream;
00414     Command::Buffer::Formatter format;
00415     ostream << format(buffer); // convert to an easily readable hex string.
00416     std_msgs::String s;
00417     s.data = ostream.str();
00418     if (ros::ok())
00419     {
00420       raw_data_command_publisher.publish(s);
00421     }
00422   }
00423 }
00437 void KobukiRos::publishRawDataStream(PacketFinder::BufferType &buffer)
00438 {
00439   if ( raw_data_stream_publisher.getNumSubscribers() > 0 ) { // do not do string processing if there is no-one listening.
00440     /*std::cout << "size: [" << buffer.size() << "], asize: [" << buffer.asize() << "]" << std::endl;
00441     std::cout << "leader: " << buffer.leader << ", follower: " << buffer.follower  << std::endl;
00442     {
00443       std::ostringstream ostream;
00444       PacketFinder::BufferType::Formatter format;
00445       ostream << format(buffer); // convert to an easily readable hex string.
00446       //std::cout << ostream.str() << std::endl;
00447       std_msgs::String s;
00448       s.data = ostream.str();
00449       if (ros::ok())
00450       {
00451         raw_data_stream_publisher.publish(s);
00452       }
00453     }*/
00454     {
00455       std::ostringstream ostream;
00456       ostream << "{ " ;
00457       ostream << std::setfill('0') << std::uppercase;
00458       for (unsigned int i=0; i < buffer.size(); i++)
00459           ostream << std::hex << std::setw(2) << static_cast<unsigned int>(buffer[i]) << " " << std::dec;
00460       ostream << "}";
00461       //std::cout << ostream.str() << std::endl;
00462       std_msgs::StringPtr msg(new std_msgs::String);
00463       msg->data = ostream.str();
00464       if (ros::ok())
00465       {
00466         raw_data_stream_publisher.publish(msg);
00467       }
00468     }
00469   }
00470 }
00471 
00472 void KobukiRos::publishRawControlCommand(const std::vector<short> &velocity_commands)
00473 {
00474   if ( raw_control_command_publisher.getNumSubscribers() > 0 ) {
00475     std_msgs::Int16MultiArrayPtr msg(new std_msgs::Int16MultiArray);
00476     msg->data = velocity_commands;
00477     if (ros::ok())
00478     {
00479       raw_control_command_publisher.publish(msg);
00480     }
00481   }
00482   return;
00483 }
00484 
00485 } // namespace kobuki


kobuki_node
Author(s): Daniel Stonier, Younghun Ju, Jorge Santos Simon
autogenerated on Thu Jun 6 2019 17:37:58