simple_loop.cpp
Go to the documentation of this file.
1 
9 /*****************************************************************************
10  * Includes
11  ****************************************************************************/
12 
13 #include <csignal>
14 #include <ecl/time.hpp>
15 #include <ecl/sigslots.hpp>
16 #include <ecl/geometry/legacy_pose2d.hpp>
17 #include <ecl/linear_algebra.hpp>
18 #include "kobuki_driver/kobuki.hpp"
19 
20 /*****************************************************************************
21 ** Classes
22 *****************************************************************************/
23 
24 class KobukiManager {
25 public:
27  dx(0.0), dth(0.0),
29  {
30  kobuki::Parameters parameters;
31  parameters.sigslots_namespace = "/kobuki";
32  parameters.device_port = "/dev/kobuki";
33  parameters.enable_acceleration_limiter = false;
34  kobuki.init(parameters);
35  kobuki.enable();
36  slot_stream_data.connect("/kobuki/stream_data");
37  }
38 
40  kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
41  kobuki.disable();
42  }
43 
45  ecl::LegacyPose2D<double> pose_update;
46  ecl::linear_algebra::Vector3d pose_update_rates;
47  kobuki.updateOdometry(pose_update, pose_update_rates);
48  pose *= pose_update;
49  dx += pose_update.x();
50  dth += pose_update.heading();
51  //std::cout << dx << ", " << dth << std::endl;
52  //std::cout << kobuki.getHeading() << ", " << pose.heading() << std::endl;
53  //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
54  processMotion();
55  }
56 
57  // Generate square motion
58  void processMotion() {
59  if (dx >= 1.0 && dth >= ecl::pi/2.0) { dx=0.0; dth=0.0; kobuki.setBaseControl(0.0, 0.0); return; }
60  else if (dx >= 1.0) { kobuki.setBaseControl(0.0, 3.3); return; }
61  else { kobuki.setBaseControl(0.3, 0.0); return; }
62  }
63 
64  ecl::LegacyPose2D<double> getPose() {
65  return pose;
66  }
67 
68 private:
69  double dx, dth;
70  ecl::LegacyPose2D<double> pose;
72  ecl::Slot<> slot_stream_data;
73 };
74 
75 /*****************************************************************************
76 ** Signal Handler
77 *****************************************************************************/
78 
79 bool shutdown_req = false;
80 void signalHandler(int signum) {
81  shutdown_req = true;
82 }
83 
84 /*****************************************************************************
85 ** Main
86 *****************************************************************************/
87 
88 int main(int argc, char** argv)
89 {
90  signal(SIGINT, signalHandler);
91 
92  std::cout << "Demo : Example of simple control loop." << std::endl;
93  KobukiManager kobuki_manager;
94 
95  ecl::Sleep sleep(1);
96  ecl::LegacyPose2D<double> pose;
97  try {
98  while (!shutdown_req){
99  sleep();
100  pose = kobuki_manager.getPose();
101  std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
102  }
103  } catch ( ecl::StandardException &e ) {
104  std::cout << e.what();
105  }
106  return 0;
107 }
void processMotion()
Definition: simple_loop.cpp:58
ecl::LegacyPose2D< double > pose
Definition: simple_loop.cpp:70
std::string device_port
The serial device port name [/dev/kobuki].
Definition: parameters.hpp:54
std::string sigslots_namespace
The first part of a sigslot connection namespace ["/kobuki"].
Definition: parameters.hpp:55
Device driver core interface.
kobuki::Kobuki kobuki
int main(int argc, char **argv)
Definition: simple_loop.cpp:88
void processStreamData()
Definition: sigslots.cpp:46
Parameter list and validator for the kobuki.
Definition: parameters.hpp:36
Keyboard remote control for our robot core (mobile base).
bool shutdown_req
Definition: simple_loop.cpp:79
The core kobuki driver class.
Definition: kobuki.hpp:87
void signalHandler(int signum)
Definition: simple_loop.cpp:80
bool enable_acceleration_limiter
Enable or disable the acceleration limiter [true].
Definition: parameters.hpp:57
ecl::LegacyPose2D< double > getPose()
Definition: simple_loop.cpp:64
ecl::Slot slot_stream_data
Definition: sigslots.cpp:53


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Mon Mar 27 2017 01:02:59