subscriber_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 "../../include/kobuki_node/kobuki_ros.hpp"
00041 
00042 /*****************************************************************************
00043  ** Namespaces
00044  *****************************************************************************/
00045 
00046 namespace kobuki
00047 {
00048 
00049 /*****************************************************************************
00050  ** Implementation
00051  *****************************************************************************/
00052 
00053 void KobukiRos::subscribeVelocityCommand(const geometry_msgs::TwistConstPtr msg)
00054 {
00055   if (kobuki.isEnabled())
00056   {
00057     // For now assuming this is in the robot frame, but probably this
00058     // should be global frame and require a transform
00059     //double vx = msg->linear.x;        // in (m/s)
00060     //double wz = msg->angular.z;       // in (rad/s)
00061     ROS_DEBUG_STREAM("Kobuki : velocity command received [" << msg->linear.x << "],[" << msg->angular.z << "]");
00062     kobuki.setBaseControl(msg->linear.x, msg->angular.z);
00063     odometry.resetTimeout();
00064   }
00065   return;
00066 }
00067 
00068 
00069 void KobukiRos::subscribeLed1Command(const kobuki_msgs::LedConstPtr msg)
00070 {
00071   switch( msg->value ) {
00072   case kobuki_msgs::Led::GREEN:  kobuki.setLed(Led1, Green ); break;
00073   case kobuki_msgs::Led::ORANGE: kobuki.setLed(Led1, Orange ); break; 
00074   case kobuki_msgs::Led::RED:    kobuki.setLed(Led1, Red ); break;
00075   case kobuki_msgs::Led::BLACK:  kobuki.setLed(Led1, Black ); break;
00076   default: ROS_WARN_STREAM("Kobuki : led 1 command value invalid."); break;
00077   }
00078   return;
00079 }
00080 
00081 void KobukiRos::subscribeLed2Command(const kobuki_msgs::LedConstPtr msg)
00082 {
00083   switch( msg->value ) {
00084   case kobuki_msgs::Led::GREEN:  kobuki.setLed(Led2, Green ); break;
00085   case kobuki_msgs::Led::ORANGE: kobuki.setLed(Led2, Orange ); break;
00086   case kobuki_msgs::Led::RED:    kobuki.setLed(Led2, Red ); break;
00087   case kobuki_msgs::Led::BLACK:  kobuki.setLed(Led2, Black ); break;
00088   default: ROS_WARN_STREAM("Kobuki : led 2 command value invalid."); break;
00089   }
00090   return;
00091 }
00092 
00093 void KobukiRos::subscribeDigitalOutputCommand(const kobuki_msgs::DigitalOutputConstPtr msg)
00094 {
00095   DigitalOutput digital_output;
00096   for ( unsigned int i = 0; i < 4; ++i ) {
00097     digital_output.values[i] = msg->values[i];
00098     digital_output.mask[i] = msg->mask[i];
00099   }
00100   kobuki.setDigitalOutput(digital_output);
00101   return;
00102 }
00103 
00104 void KobukiRos::subscribeExternalPowerCommand(const kobuki_msgs::ExternalPowerConstPtr msg)
00105 {
00106   // Validate message
00107   if (!((msg->source == kobuki_msgs::ExternalPower::PWR_3_3V1A) ||
00108         (msg->source == kobuki_msgs::ExternalPower::PWR_5V1A) ||
00109         (msg->source == kobuki_msgs::ExternalPower::PWR_12V5A) ||
00110         (msg->source == kobuki_msgs::ExternalPower::PWR_12V1_5A)))
00111   {
00112     ROS_ERROR_STREAM("Kobuki : Power source " << (unsigned int)msg->source << " does not exist! [" << name << "].");
00113     return;
00114   }
00115   if (!((msg->state == kobuki_msgs::ExternalPower::OFF) ||
00116       (msg->state == kobuki_msgs::ExternalPower::ON)))
00117   {
00118     ROS_ERROR_STREAM("Kobuki : Power source state "
00119         << (unsigned int)msg->state << " does not exist! [" << name << "].");
00120     return;
00121   }
00122 
00123   DigitalOutput digital_output;
00124   for ( unsigned int i = 0; i < 4; ++i )
00125   {
00126     if (i == msg->source)
00127     {
00128       if (msg->state)
00129       {
00130         digital_output.values[i] = true; // turn source on
00131         ROS_INFO_STREAM("Kobuki : Turning on external power source "
00132             << (unsigned int)msg->source << ". [" << name << "].");
00133       }
00134       else
00135       {
00136         digital_output.values[i] = false; // turn source off
00137         ROS_INFO_STREAM("Kobuki : Turning off external power source "
00138             << (unsigned int)msg->source << ". [" << name << "].");
00139       }
00140       digital_output.mask[i] = true; // change source state
00141     }
00142     else
00143     {
00144       digital_output.values[i] = false; // values doesn't matter here, since mask is set false, what means ignoring
00145       digital_output.mask[i] = false;
00146     }
00147   }
00148   kobuki.setExternalPower(digital_output);
00149   return;
00150 }
00151 
00155 void KobukiRos::subscribeSoundCommand(const kobuki_msgs::SoundConstPtr msg)
00156 {
00157   if ( msg->value == kobuki_msgs::Sound::ON )
00158   {
00159     kobuki.playSoundSequence(On);
00160   }
00161   else if ( msg->value == kobuki_msgs::Sound::OFF )
00162   {
00163     kobuki.playSoundSequence(Off);
00164   }
00165   else if ( msg->value == kobuki_msgs::Sound::RECHARGE )
00166   {
00167     kobuki.playSoundSequence(Recharge);
00168   }
00169   else if ( msg->value == kobuki_msgs::Sound::BUTTON )
00170   {
00171     kobuki.playSoundSequence(Button);
00172   }
00173   else if ( msg->value == kobuki_msgs::Sound::ERROR )
00174   {
00175     kobuki.playSoundSequence(Error);
00176   }
00177   else if ( msg->value == kobuki_msgs::Sound::CLEANINGSTART )
00178   {
00179     kobuki.playSoundSequence(CleaningStart);
00180   }
00181   else if ( msg->value == kobuki_msgs::Sound::CLEANINGEND )
00182   {
00183     kobuki.playSoundSequence(CleaningEnd);
00184   }
00185   else
00186   {
00187     ROS_WARN_STREAM("Kobuki : Invalid sound command! There is no sound stored for value '" << msg->value << "'.");
00188   }
00189   return;
00190 }
00191 
00195 void KobukiRos::subscribeResetOdometry(const std_msgs::EmptyConstPtr /* msg */)
00196 {
00197   ROS_INFO_STREAM("Kobuki : Resetting the odometry. [" << name << "].");
00198   joint_states.position[0] = 0.0; // wheel_left
00199   joint_states.velocity[0] = 0.0;
00200   joint_states.position[1] = 0.0; // wheel_right
00201   joint_states.velocity[1] = 0.0;
00202   odometry.resetOdometry();
00203   kobuki.resetOdometry();
00204   return;
00205 }
00206 
00207 void KobukiRos::subscribeMotorPower(const kobuki_msgs::MotorPowerConstPtr msg)
00208 {
00209   if (msg->state == kobuki_msgs::MotorPower::ON)
00210   {
00211     ROS_INFO_STREAM("Kobuki : Firing up the motors. [" << name << "]");
00212     kobuki.enable();
00213     odometry.resetTimeout();
00214   }
00215   else if (msg->state == kobuki_msgs::MotorPower::OFF)
00216   {
00217     kobuki.disable();
00218     ROS_INFO_STREAM("Kobuki : Shutting down the motors. [" << name << "]");
00219     odometry.resetTimeout();
00220   }
00221   else
00222   {
00223     ROS_ERROR_STREAM("Kobuki : Motor power command specifies unknown state '" << (unsigned int)msg->state
00224                      << "'. [" << name << "]");
00225   }
00226 }
00227 
00228 void KobukiRos::subscribeControllerInfoCommand(const kobuki_msgs::ControllerInfoConstPtr msg)
00229 {
00230   if( msg->p_gain < 0.0f ||  msg->i_gain < 0.0f ||  msg->d_gain < 0.0f) {
00231     ROS_ERROR_STREAM("Kobuki : All controller gains should be positive. [" << name << "]");
00232     return;
00233   }
00234   kobuki.setControllerGain(msg->type,
00235                            static_cast<unsigned int>(msg->p_gain*1000.0f),
00236                            static_cast<unsigned int>(msg->i_gain*1000.0f),
00237                            static_cast<unsigned int>(msg->d_gain*1000.0f));
00238   return;
00239 }
00240 
00241 } // namespace kobuki


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