kobuki_ros.hpp
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  ** Ifdefs
00038  *****************************************************************************/
00039 
00040 #ifndef KOBUKI_ROS_HPP_
00041 #define KOBUKI_ROS_HPP_
00042 
00043 /*****************************************************************************
00044  ** Includes
00045  *****************************************************************************/
00046 
00047 #include <string>
00048 #include <boost/shared_ptr.hpp>
00049 
00050 #include <ros/ros.h>
00051 #include <angles/angles.h>
00052 #include <std_msgs/Empty.h>
00053 #include <std_msgs/String.h>
00054 #include <std_msgs/Int16MultiArray.h>
00055 #include <sensor_msgs/JointState.h>
00056 #include <sensor_msgs/Imu.h>
00057 #include <ecl/sigslots.hpp>
00058 #include <kobuki_msgs/ButtonEvent.h>
00059 #include <kobuki_msgs/BumperEvent.h>
00060 #include <kobuki_msgs/CliffEvent.h>
00061 #include <kobuki_msgs/ControllerInfo.h>
00062 #include <kobuki_msgs/DigitalOutput.h>
00063 #include <kobuki_msgs/DigitalInputEvent.h>
00064 #include <kobuki_msgs/ExternalPower.h>
00065 #include <kobuki_msgs/DockInfraRed.h>
00066 #include <kobuki_msgs/Led.h>
00067 #include <kobuki_msgs/MotorPower.h>
00068 #include <kobuki_msgs/PowerSystemEvent.h>
00069 #include <kobuki_msgs/RobotStateEvent.h>
00070 #include <kobuki_msgs/SensorState.h>
00071 #include <kobuki_msgs/Sound.h>
00072 #include <kobuki_msgs/VersionInfo.h>
00073 #include <kobuki_msgs/WheelDropEvent.h>
00074 #include <kobuki_driver/kobuki.hpp>
00075 #include "diagnostics.hpp"
00076 #include "odometry.hpp"
00077 
00078 /*****************************************************************************
00079  ** Namespaces
00080  *****************************************************************************/
00081 
00082 namespace kobuki
00083 {
00084 class KobukiRos
00085 {
00086 public:
00087   KobukiRos(std::string& node_name);
00088   ~KobukiRos();
00089   bool init(ros::NodeHandle& nh, ros::NodeHandle& nh_pub);
00090   bool update();
00091 
00092   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00093 private:
00094   /*********************
00095    ** Variables
00096    **********************/
00097   std::string name; // name of the ROS node
00098   Kobuki kobuki;
00099   sensor_msgs::JointState joint_states;
00100   Odometry odometry;
00101   bool cmd_vel_timed_out_; // stops warning spam when cmd_vel flags as timed out more than once in a row
00102   bool serial_timed_out_; // stops warning spam when serial connection timed out more than once in a row
00103 
00104   /*********************
00105    ** Ros Comms
00106    **********************/
00107   ros::Publisher version_info_publisher, controller_info_publisher;
00108   ros::Publisher imu_data_publisher, sensor_state_publisher, joint_state_publisher, dock_ir_publisher, raw_imu_data_publisher;
00109   ros::Publisher button_event_publisher, input_event_publisher, robot_event_publisher;
00110   ros::Publisher bumper_event_publisher, cliff_event_publisher, wheel_event_publisher, power_event_publisher;
00111   ros::Publisher raw_data_command_publisher, raw_data_stream_publisher, raw_control_command_publisher;
00112 
00113   ros::Subscriber velocity_command_subscriber, digital_output_command_subscriber, external_power_command_subscriber;
00114   ros::Subscriber controller_info_command_subscriber;
00115   ros::Subscriber led1_command_subscriber, led2_command_subscriber, sound_command_subscriber;
00116   ros::Subscriber motor_power_subscriber, reset_odometry_subscriber;
00117 
00118   void advertiseTopics(ros::NodeHandle& nh);
00119   void subscribeTopics(ros::NodeHandle& nh);
00120 
00121   /*********************
00122   ** Ros Callbacks
00123   **********************/
00124   void subscribeVelocityCommand(const geometry_msgs::TwistConstPtr);
00125   void subscribeLed1Command(const kobuki_msgs::LedConstPtr);
00126   void subscribeLed2Command(const kobuki_msgs::LedConstPtr);
00127   void subscribeDigitalOutputCommand(const kobuki_msgs::DigitalOutputConstPtr);
00128   void subscribeExternalPowerCommand(const kobuki_msgs::ExternalPowerConstPtr);
00129   void subscribeResetOdometry(const std_msgs::EmptyConstPtr);
00130   void subscribeSoundCommand(const kobuki_msgs::SoundConstPtr);
00131   void subscribeMotorPower(const kobuki_msgs::MotorPowerConstPtr msg);
00132   void subscribeControllerInfoCommand(const kobuki_msgs::ControllerInfoConstPtr msg);
00133 
00134   /*********************
00135    ** SigSlots
00136    **********************/
00137   ecl::Slot<const VersionInfo&> slot_version_info;
00138   ecl::Slot<> slot_stream_data;
00139   ecl::Slot<> slot_controller_info;
00140   ecl::Slot<const ButtonEvent&> slot_button_event;
00141   ecl::Slot<const BumperEvent&> slot_bumper_event;
00142   ecl::Slot<const CliffEvent&>  slot_cliff_event;
00143   ecl::Slot<const WheelEvent&>  slot_wheel_event;
00144   ecl::Slot<const PowerEvent&>  slot_power_event;
00145   ecl::Slot<const InputEvent&>  slot_input_event;
00146   ecl::Slot<const RobotEvent&>  slot_robot_event;
00147   ecl::Slot<const std::string&> slot_debug, slot_info, slot_warn, slot_error;
00148   ecl::Slot<const std::vector<std::string>&> slot_named;
00149   ecl::Slot<Command::Buffer&> slot_raw_data_command;
00150   ecl::Slot<PacketFinder::BufferType&> slot_raw_data_stream;
00151   ecl::Slot<const std::vector<short>&> slot_raw_control_command;
00152 
00153   /*********************
00154    ** Slot Callbacks
00155    **********************/
00156   void processStreamData();
00157   void publishWheelState();
00158   void publishInertia();
00159   void publishRawInertia();
00160   void publishSensorState();
00161   void publishDockIRData();
00162   void publishVersionInfo(const VersionInfo &version_info);
00163   void publishControllerInfo();
00164   void publishButtonEvent(const ButtonEvent &event);
00165   void publishBumperEvent(const BumperEvent &event);
00166   void publishCliffEvent(const CliffEvent &event);
00167   void publishWheelEvent(const WheelEvent &event);
00168   void publishPowerEvent(const PowerEvent &event);
00169   void publishInputEvent(const InputEvent &event);
00170   void publishRobotEvent(const RobotEvent &event);
00171 
00172 
00173   // debugging
00174   void rosDebug(const std::string &msg) { ROS_DEBUG_STREAM("Kobuki : " << msg); }
00175   void rosInfo(const std::string &msg) { ROS_INFO_STREAM("Kobuki : " << msg); }
00176   void rosWarn(const std::string &msg) { ROS_WARN_STREAM("Kobuki : " << msg); }
00177   void rosError(const std::string &msg) { ROS_ERROR_STREAM("Kobuki : " << msg); }
00178   void rosNamed(const std::vector<std::string> &msgs) {
00179     if (msgs.size()==0) return;
00180     if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); }
00181     if (msgs.size()==2) {
00182       if      (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); }
00183       else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); }
00184       else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); }
00185       else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); }
00186       else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); }
00187     }
00188     if (msgs.size()==3) {
00189       if      (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
00190       else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
00191       else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
00192       else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
00193       else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
00194     }
00195   }
00196 
00197   void publishRawDataCommand(Command::Buffer &buffer);
00198   void publishRawDataStream(PacketFinder::BufferType &buffer);
00199   void publishRawControlCommand(const std::vector<short> &velocity_commands);
00200 
00201   /*********************
00202   ** Diagnostics
00203   **********************/
00204   diagnostic_updater::Updater updater;
00205   BatteryTask     battery_diagnostics;
00206   WatchdogTask   watchdog_diagnostics;
00207   CliffSensorTask   cliff_diagnostics;
00208   WallSensorTask   bumper_diagnostics;
00209   WheelDropTask     wheel_diagnostics;
00210   MotorCurrentTask  motor_diagnostics;
00211   MotorStateTask    state_diagnostics;
00212   GyroSensorTask     gyro_diagnostics;
00213   DigitalInputTask dinput_diagnostics;
00214   AnalogInputTask  ainput_diagnostics;
00215 };
00216 
00217 } // namespace kobuki
00218 
00219 #endif /* KOBUKI_ROS_HPP_ */


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