simple_loop.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  * Includes
00011  ****************************************************************************/
00012 
00013 #include <csignal>
00014 #include <ecl/time.hpp>
00015 #include <ecl/sigslots.hpp>
00016 #include <ecl/geometry/legacy_pose2d.hpp>
00017 #include <ecl/linear_algebra.hpp>
00018 #include "kobuki_driver/kobuki.hpp"
00019 
00020 /*****************************************************************************
00021 ** Classes
00022 *****************************************************************************/
00023 
00024 class KobukiManager {
00025 public:
00026   KobukiManager() :
00027     dx(0.0), dth(0.0),
00028     slot_stream_data(&KobukiManager::processStreamData, *this)
00029   {
00030     kobuki::Parameters parameters;
00031     parameters.sigslots_namespace = "/kobuki";
00032     parameters.device_port = "/dev/kobuki";
00033     parameters.enable_acceleration_limiter = false;
00034     kobuki.init(parameters);
00035     kobuki.enable();
00036     slot_stream_data.connect("/kobuki/stream_data");
00037   }
00038 
00039   ~KobukiManager() {
00040     kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
00041     kobuki.disable();
00042   }
00043 
00044   void processStreamData() {
00045     ecl::LegacyPose2D<double> pose_update;
00046     ecl::linear_algebra::Vector3d pose_update_rates;
00047     kobuki.updateOdometry(pose_update, pose_update_rates);
00048     pose *= pose_update;
00049     dx += pose_update.x();
00050     dth += pose_update.heading();
00051     //std::cout << dx << ", " << dth << std::endl;
00052     //std::cout << kobuki.getHeading() << ", " << pose.heading() << std::endl;
00053     //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
00054     processMotion();
00055   }
00056 
00057   // Generate square motion
00058   void processMotion() {
00059     if (dx >= 1.0 && dth >= ecl::pi/2.0) { dx=0.0; dth=0.0; kobuki.setBaseControl(0.0, 0.0); return; }
00060     else if (dx >= 1.0) { kobuki.setBaseControl(0.0, 3.3); return; }
00061     else { kobuki.setBaseControl(0.3, 0.0); return; }
00062   }
00063 
00064   ecl::LegacyPose2D<double> getPose() {
00065     return pose;
00066   }
00067 
00068 private:
00069   double dx, dth;
00070   ecl::LegacyPose2D<double> pose;
00071   kobuki::Kobuki kobuki;
00072   ecl::Slot<> slot_stream_data;
00073 };
00074 
00075 /*****************************************************************************
00076 ** Signal Handler
00077 *****************************************************************************/
00078 
00079 bool shutdown_req = false;
00080 void signalHandler(int signum) {
00081   shutdown_req = true;
00082 }
00083 
00084 /*****************************************************************************
00085 ** Main
00086 *****************************************************************************/
00087 
00088 int main(int argc, char** argv)
00089 {
00090   signal(SIGINT, signalHandler);
00091 
00092   std::cout << "Demo : Example of simple control loop." << std::endl;
00093   KobukiManager kobuki_manager;
00094 
00095   ecl::Sleep sleep(1);
00096   ecl::LegacyPose2D<double> pose;
00097   try {
00098     while (!shutdown_req){
00099       sleep();
00100       pose = kobuki_manager.getPose();
00101       std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
00102     }
00103   } catch ( ecl::StandardException &e ) {
00104     std::cout << e.what();
00105   }
00106   return 0;
00107 }


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37