kobuki_ros.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  */
00035 /*****************************************************************************
00036  ** Includes
00037  *****************************************************************************/
00038 
00039 #include <float.h>
00040 #include <tf/tf.h>
00041 #include <ecl/streams/string_stream.hpp>
00042 #include <kobuki_msgs/VersionInfo.h>
00043 #include "kobuki_node/kobuki_ros.hpp"
00044 
00045 /*****************************************************************************
00046  ** Namespaces
00047  *****************************************************************************/
00048 
00049 namespace kobuki
00050 {
00051 
00052 /*****************************************************************************
00053  ** Implementation [KobukiRos]
00054  *****************************************************************************/
00055 
00061 KobukiRos::KobukiRos(std::string& node_name) :
00062     name(node_name), cmd_vel_timed_out_(false), serial_timed_out_(false),
00063     slot_version_info(&KobukiRos::publishVersionInfo, *this),
00064     slot_controller_info(&KobukiRos::publishControllerInfo, *this),
00065     slot_stream_data(&KobukiRos::processStreamData, *this),
00066     slot_button_event(&KobukiRos::publishButtonEvent, *this),
00067     slot_bumper_event(&KobukiRos::publishBumperEvent, *this),
00068     slot_cliff_event(&KobukiRos::publishCliffEvent, *this),
00069     slot_wheel_event(&KobukiRos::publishWheelEvent, *this),
00070     slot_power_event(&KobukiRos::publishPowerEvent, *this),
00071     slot_input_event(&KobukiRos::publishInputEvent, *this),
00072     slot_robot_event(&KobukiRos::publishRobotEvent, *this),
00073     slot_debug(&KobukiRos::rosDebug, *this),
00074     slot_info(&KobukiRos::rosInfo, *this),
00075     slot_warn(&KobukiRos::rosWarn, *this),
00076     slot_error(&KobukiRos::rosError, *this),
00077     slot_named(&KobukiRos::rosNamed, *this),
00078     slot_raw_data_command(&KobukiRos::publishRawDataCommand, *this),
00079     slot_raw_data_stream(&KobukiRos::publishRawDataStream, *this),
00080     slot_raw_control_command(&KobukiRos::publishRawControlCommand, *this)
00081 {
00082   updater.setHardwareID("Kobuki");
00083   updater.add(battery_diagnostics);
00084   updater.add(watchdog_diagnostics);
00085   updater.add(bumper_diagnostics);
00086   updater.add(cliff_diagnostics);
00087   updater.add(wheel_diagnostics);
00088   updater.add(motor_diagnostics);
00089   updater.add(state_diagnostics);
00090   updater.add(gyro_diagnostics);
00091   updater.add(dinput_diagnostics);
00092   updater.add(ainput_diagnostics);
00093 }
00094 
00099 KobukiRos::~KobukiRos()
00100 {
00101   ROS_INFO_STREAM("Kobuki : waiting for kobuki thread to finish [" << name << "].");
00102 }
00103 
00104 bool KobukiRos::init(ros::NodeHandle& nh, ros::NodeHandle& nh_pub)
00105 {
00106   /*********************
00107    ** Communications
00108    **********************/
00109   advertiseTopics(nh);
00110   subscribeTopics(nh);
00111 
00112   /*********************
00113    ** Slots
00114    **********************/
00115   slot_stream_data.connect(name + std::string("/stream_data"));
00116   slot_version_info.connect(name + std::string("/version_info"));
00117   slot_controller_info.connect(name + std::string("/controller_info"));
00118   slot_button_event.connect(name + std::string("/button_event"));
00119   slot_bumper_event.connect(name + std::string("/bumper_event"));
00120   slot_cliff_event.connect(name + std::string("/cliff_event"));
00121   slot_wheel_event.connect(name + std::string("/wheel_event"));
00122   slot_power_event.connect(name + std::string("/power_event"));
00123   slot_input_event.connect(name + std::string("/input_event"));
00124   slot_robot_event.connect(name + std::string("/robot_event"));
00125   slot_debug.connect(name + std::string("/ros_debug"));
00126   slot_info.connect(name + std::string("/ros_info"));
00127   slot_warn.connect(name + std::string("/ros_warn"));
00128   slot_error.connect(name + std::string("/ros_error"));
00129   slot_named.connect(name + std::string("/ros_named"));
00130   slot_raw_data_command.connect(name + std::string("/raw_data_command"));
00131   slot_raw_data_stream.connect(name + std::string("/raw_data_stream"));
00132   slot_raw_control_command.connect(name + std::string("/raw_control_command"));
00133 
00134   /*********************
00135    ** Driver Parameters
00136    **********************/
00137   Parameters parameters;
00138 
00139   nh.param("acceleration_limiter", parameters.enable_acceleration_limiter, false);
00140   nh.param("battery_capacity", parameters.battery_capacity, Battery::capacity);
00141   nh.param("battery_low", parameters.battery_low, Battery::low);
00142   nh.param("battery_dangerous", parameters.battery_dangerous, Battery::dangerous);
00143 
00144   parameters.sigslots_namespace = name; // name is automatically picked up by device_nodelet parent.
00145   if (!nh.getParam("device_port", parameters.device_port))
00146   {
00147     ROS_ERROR_STREAM("Kobuki : no device port given on the parameter server (e.g. /dev/ttyUSB0)[" << name << "].");
00148     return false;
00149   }
00150 
00151   /*********************
00152    ** Joint States
00153    **********************/
00154   std::string robot_description, wheel_left_joint_name, wheel_right_joint_name;
00155 
00156   nh.param("wheel_left_joint_name", wheel_left_joint_name, std::string("wheel_left_joint"));
00157   nh.param("wheel_right_joint_name", wheel_right_joint_name, std::string("wheel_right_joint"));
00158 
00159   // minimalistic check: are joint names present on robot description file?
00160   if (!nh_pub.getParam("robot_description", robot_description))
00161   {
00162     ROS_WARN("Kobuki : no robot description given on the parameter server");
00163   }
00164   else
00165   {
00166     if (robot_description.find(wheel_left_joint_name) == std::string::npos) {
00167       ROS_WARN("Kobuki : joint name %s not found on robot description", wheel_left_joint_name.c_str());
00168     }
00169 
00170     if (robot_description.find(wheel_right_joint_name) == std::string::npos) {
00171       ROS_WARN("Kobuki : joint name %s not found on robot description", wheel_right_joint_name.c_str());
00172     }
00173   }
00174   joint_states.name.push_back(wheel_left_joint_name);
00175   joint_states.name.push_back(wheel_right_joint_name);
00176   joint_states.position.resize(2,0.0);
00177   joint_states.velocity.resize(2,0.0);
00178   joint_states.effort.resize(2,0.0);
00179 
00180   /*********************
00181    ** Validation
00182    **********************/
00183   if (!parameters.validate())
00184   {
00185     ROS_ERROR_STREAM("Kobuki : parameter configuration failed [" << name << "].");
00186     ROS_ERROR_STREAM("Kobuki : " << parameters.error_msg << "[" << name << "]");
00187     return false;
00188   }
00189   else
00190   {
00191     if (parameters.simulation)
00192     {
00193       ROS_INFO("Kobuki : driver going into loopback (simulation) mode.");
00194     }
00195     else
00196     {
00197       ROS_INFO_STREAM("Kobuki : configured for connection on device_port "
00198                       << parameters.device_port << " [" << name << "].");
00199       ROS_INFO_STREAM("Kobuki : driver running in normal (non-simulation) mode" << " [" << name << "].");
00200     }
00201   }
00202 
00203   odometry.init(nh, name);
00204 
00205   /*********************
00206    ** Driver Init
00207    **********************/
00208   try
00209   {
00210     kobuki.init(parameters);
00211     ros::Duration(0.25).sleep(); // wait for some data to come in.
00212     if ( !kobuki.isAlive() ) {
00213       ROS_WARN_STREAM("Kobuki : no data stream, is kobuki turned on?");
00214       // don't need to return false here - simply turning kobuki on while spin()'ing should resurrect the situation.
00215     }
00216     kobuki.enable();
00217   }
00218   catch (const ecl::StandardException &e)
00219   {
00220     switch (e.flag())
00221     {
00222       case (ecl::OpenError):
00223       {
00224         ROS_ERROR_STREAM("Kobuki : could not open connection [" << parameters.device_port << "][" << name << "].");
00225         break;
00226       }
00227       default:
00228       {
00229         ROS_ERROR_STREAM("Kobuki : initialisation failed [" << name << "].");
00230         ROS_DEBUG_STREAM(e.what());
00231         break;
00232       }
00233     }
00234     return false;
00235   }
00236   // kobuki.printSigSlotConnections();
00237   return true;
00238 }
00248 bool KobukiRos::update()
00249 {
00250   if ( kobuki.isShutdown() )
00251   {
00252     ROS_ERROR_STREAM("Kobuki : Driver has been shutdown. Stopping update loop. [" << name << "].");
00253     return false;
00254   }
00255 
00256   if ( (kobuki.isEnabled() == true) && odometry.commandTimeout())
00257   {
00258     if ( !cmd_vel_timed_out_ )
00259     {
00260       kobuki.setBaseControl(0, 0);
00261       cmd_vel_timed_out_ = true;
00262       ROS_WARN("Kobuki : Incoming velocity commands not received for more than %.2f seconds -> zero'ing velocity commands", odometry.timeout().toSec());
00263     }
00264   }
00265   else
00266   {
00267     cmd_vel_timed_out_ = false;
00268   }
00269 
00270   bool is_alive = kobuki.isAlive();
00271   if ( watchdog_diagnostics.isAlive() && !is_alive )
00272   {
00273     if ( !serial_timed_out_ )
00274     {
00275       ROS_ERROR_STREAM("Kobuki : Timed out while waiting for serial data stream [" << name << "].");
00276       serial_timed_out_ = true;
00277     }
00278     else
00279     {
00280       serial_timed_out_ = false;
00281     }
00282   }
00283 
00284   watchdog_diagnostics.update(is_alive);
00285   battery_diagnostics.update(kobuki.batteryStatus());
00286   cliff_diagnostics.update(kobuki.getCoreSensorData().cliff, kobuki.getCliffData());
00287   bumper_diagnostics.update(kobuki.getCoreSensorData().bumper);
00288   wheel_diagnostics.update(kobuki.getCoreSensorData().wheel_drop);
00289   motor_diagnostics.update(kobuki.getCurrentData().current);
00290   state_diagnostics.update(kobuki.isEnabled());
00291   gyro_diagnostics.update(kobuki.getInertiaData().angle);
00292   dinput_diagnostics.update(kobuki.getGpInputData().digital_input);
00293   ainput_diagnostics.update(kobuki.getGpInputData().analog_input);
00294   updater.update();
00295 
00296   return true;
00297 }
00298 
00303 void KobukiRos::advertiseTopics(ros::NodeHandle& nh)
00304 {
00305   /*********************
00306   ** Turtlebot Required
00307   **********************/
00308   joint_state_publisher = nh.advertise <sensor_msgs::JointState>("joint_states",100);
00309 
00310   /*********************
00311   ** Kobuki Esoterics
00312   **********************/
00313   version_info_publisher = nh.advertise < kobuki_msgs::VersionInfo > ("version_info",  100, true); // latched publisher
00314   controller_info_publisher = nh.advertise < kobuki_msgs::ControllerInfo > ("controller_info",  100, true); // latched publisher
00315   button_event_publisher = nh.advertise < kobuki_msgs::ButtonEvent > ("events/button", 100);
00316   bumper_event_publisher = nh.advertise < kobuki_msgs::BumperEvent > ("events/bumper", 100);
00317   cliff_event_publisher  = nh.advertise < kobuki_msgs::CliffEvent >  ("events/cliff",  100);
00318   wheel_event_publisher  = nh.advertise < kobuki_msgs::WheelDropEvent > ("events/wheel_drop", 100);
00319   power_event_publisher  = nh.advertise < kobuki_msgs::PowerSystemEvent > ("events/power_system", 100);
00320   input_event_publisher  = nh.advertise < kobuki_msgs::DigitalInputEvent > ("events/digital_input", 100);
00321   robot_event_publisher  = nh.advertise < kobuki_msgs::RobotStateEvent > ("events/robot_state", 100, true); // also latched
00322   sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);
00323   dock_ir_publisher = nh.advertise < kobuki_msgs::DockInfraRed > ("sensors/dock_ir", 100);
00324   imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data", 100);
00325   raw_imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data_raw", 100);
00326   raw_data_command_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_command", 100);
00327   raw_data_stream_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_stream", 100);
00328   raw_control_command_publisher = nh.advertise< std_msgs::Int16MultiArray > ("debug/raw_control_command", 100);
00329 }
00330 
00335 void KobukiRos::subscribeTopics(ros::NodeHandle& nh)
00336 {
00337   velocity_command_subscriber = nh.subscribe(std::string("commands/velocity"), 10, &KobukiRos::subscribeVelocityCommand, this);
00338   led1_command_subscriber =  nh.subscribe(std::string("commands/led1"), 10, &KobukiRos::subscribeLed1Command, this);
00339   led2_command_subscriber =  nh.subscribe(std::string("commands/led2"), 10, &KobukiRos::subscribeLed2Command, this);
00340   digital_output_command_subscriber =  nh.subscribe(std::string("commands/digital_output"), 10, &KobukiRos::subscribeDigitalOutputCommand, this);
00341   external_power_command_subscriber =  nh.subscribe(std::string("commands/external_power"), 10, &KobukiRos::subscribeExternalPowerCommand, this);
00342   sound_command_subscriber =  nh.subscribe(std::string("commands/sound"), 10, &KobukiRos::subscribeSoundCommand, this);
00343   reset_odometry_subscriber = nh.subscribe("commands/reset_odometry", 10, &KobukiRos::subscribeResetOdometry, this);
00344   motor_power_subscriber = nh.subscribe("commands/motor_power", 10, &KobukiRos::subscribeMotorPower, this);
00345   controller_info_command_subscriber =  nh.subscribe(std::string("commands/controller_info"), 10, &KobukiRos::subscribeControllerInfoCommand, this);
00346 }
00347 
00348 
00349 } // namespace kobuki
00350 


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