Simple Control Loop Example

Overview

This example will show you how to process kobuki's pose data and send wheel commands to robot.

Source Code

Here is full source code of simple control loop example. You can find it from src/test/simple_loop.cpp

/*****************************************************************************
* Includes
*/
#include <csignal>
#include <ecl/time.hpp>
#include <ecl/sigslots.hpp>
/*****************************************************************************
* Classes
*/
public:
dx(0.0), dth(0.0),
{
kobuki::Parameters parameters;
parameters.sigslots_namespace = "/kobuki";
parameters.device_port = "/dev/kobuki";
parameters.enable_acceleration_limiter = false;
kobuki.init(parameters);
kobuki.enable();
slot_stream_data.connect("/kobuki/stream_data");
}
kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
kobuki.disable();
}
ecl::linear_algebra::Vector3d pose_update_rates;
kobuki.updateOdometry(pose_update, pose_update_rates);
pose *= pose_update;
dx += pose_update.x();
dth += pose_update.heading();
}
// Generate square motion
void processMotion() {
if (dx >= 1.0 && dth >= ecl::pi/2.0) { dx=0.0; dth=0.0; kobuki.setBaseControl(0.0, 0.0); return; }
else if (dx >= 1.0) { kobuki.setBaseControl(0.0, 3.3); return; }
else { kobuki.setBaseControl(0.3, 0.0); return; }
}
return pose;
}
private:
double dx, dth;
};
/*****************************************************************************
* Signal Handler
*/
bool shutdown_req = false;
void signalHandler(int signum) {
shutdown_req = true;
}
/*****************************************************************************
* Main
*/
int main(int argc, char** argv)
{
signal(SIGINT, signalHandler);
std::cout << "Demo : Example of simple control loop." << std::endl;
KobukiManager kobuki_manager;
ecl::Sleep sleep(1);
try {
while (!shutdown_req){
sleep();
pose = kobuki_manager.getPose();
std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
}
} catch ( ecl::StandardException &e ) {
std::cout << e.what();
}
return 0;
}

Explanation

This simple control loop program process kobuki's stream data to retrive how far moved from last stream, and accumulate it to get current pose(position and orientation). And control the robot to following the simple squre path where each side is 1.0 meter.

Includes

#include <csignal>
#include <ecl/time.hpp>
#include <ecl/sigslots.hpp>
#include <ecl/geometry/pose2d.hpp>

ecl::Sleep from ecl/time.hpp is used to throttle the rate of outputs of current pose printing. Signal handler from csignal also introduced to prevent unexpected behavior, when it quiting.

ecl/geometry/pose2d.hpp and ecl/linear_algebra.hpp is used to store and calculated robot pose from odometry info.

Class

The KobukiManager class contains kobuki::Kobuki class, pose data and sigslot callback. KobukiManager class is extended from previous sigslots examples. Now it has dx, dth and pose data.

public:
dx(0.0), dth(0.0),
{
kobuki::Parameters parameters;
parameters.sigslots_namespace = "/kobuki";
parameters.device_port = "/dev/kobuki";
parameters.enable_acceleration_limiter = false;
kobuki.init(parameters);
kobuki.enable();
slot_stream_data.connect("/kobuki/stream_data");
}

Only kobuki.enable() is added from previous sigslot exmaple. It will turn on motor power of kobuki. To moving kobuki, you should call this function before commanding wheel velocities.

kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
kobuki.disable();
}

Destructor is added in this example. When quiting the porgam, this code will immediately stop the robot and disable it. It will improve the controllability in emergent condition, and will prevent unwanted behavior.

kobuki.setBaseControl() is a function to send wheel command to robot. It's arguments are linear and angular velocities in (m/s) and (rad/s).

disable() will turned off the motor power.

void processStreamData() {
ecl::linear_algebra::Vector3d pose_update_rates;
kobuki.updateOdometry(pose_update, pose_update_rates);
pose *= pose_update;
dx += pose_update.x();
dth += pose_update.heading();
processMotion();
}

processStreamData() is slot callback function, when stream data is arrived from robot, this callback will be called.

@ kobuki::Kobuki::updateOdometry() method is used to retriving odometry data from last stream data. It returns updated_pose and updated_pose_rate that calculated from encoder ticks.

pose_update containts calculated linear and angular displacements between the stream. pose_update_rates contains calculated linear and angular velocities as well.

And, by perform below calculation:

pose *= pose_update;

we can get accumulated pose from odoemtry.

// Generate square motion
void processMotion() {
if (dx >= 1.0 && dth >= ecl::pi/2.0) { dx=0.0; dth=0.0; kobuki.setBaseControl(0.0, 0.0); return; }
else if (dx >= 1.0) { kobuki.setBaseControl(0.0, 3.3); return; }
else { kobuki.setBaseControl(0.3, 0.0); return; }
}

processMotion() determines next base command to the robot. These commands will makes robot to follow squre path at the last, by running below commands repeatedly:

  • go forward 1.0 meter
  • rotate counter-clockwise in 90 degree

getPose() is simple getter method for accumulated pose of robot.

private:
double dx, dth;
ecl::Slot<> slot_stream_data;
};

Signal Handler

bool shutdown_req = false;
void signalHandler(int signum) {
shutdown_req = true;
}

This part is signal handler. signalHandler() will be called when the SIGINT signal is catched(e.g. user pressed the ctrl+c keys). A bool variable, shutdown_req, setted to false will break the while loop in main function, and terminates program also. If there is no signal handler, program will terminated without calling destructor of KobukiManager class. And the robot will run last wheel command continuously until timed out(5 seconds by default).

Main

int main(int argc, char** argv)
{
signal(SIGINT, signalHandler);

Call signal() function to install signalHandler().

std::cout << "Demo : Example of simple control loop." << std::endl;
KobukiManager kobuki_manager;
ecl::Sleep sleep(1);
try {
while (!shutdown_req){
sleep();
pose = kobuki_manager.getPose();
std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
}
} catch ( ecl::StandardException &e ) {
std::cout << e.what();
}
return 0;
}

Accumulated pose will be printed in every seconds. If you pressed ctrl+c keys, then robot will stop and program will be terminated.

More Complicated Example

A far more detailed implementation can be found for the kobuki_node that runs within the ROS framework.

kobuki::Parameters::sigslots_namespace
std::string sigslots_namespace
The first part of a sigslot connection namespace ["/kobuki"].
Definition: parameters.hpp:69
signalHandler
void signalHandler(int signum)
Definition: simple_loop.cpp:80
ecl::Slot
KobukiManager::processMotion
void processMotion()
Definition: simple_loop.cpp:62
kobuki::Parameters::enable_acceleration_limiter
bool enable_acceleration_limiter
Enable or disable the acceleration limiter [true].
Definition: parameters.hpp:71
kobuki
Definition: command.hpp:31
KobukiManager::dth
double dth
Definition: simple_loop.cpp:73
ecl::pi
const double pi
sigslots.hpp
shutdown_req
bool shutdown_req
Definition: simple_loop.cpp:79
KobukiManager::dx
double dx
Definition: simple_loop.cpp:73
linear_algebra.hpp
ecl::LegacyPose2D< double >
KobukiManager::getPose
ecl::LegacyPose2D< double > getPose()
Definition: simple_loop.cpp:68
KobukiManager::pose
ecl::LegacyPose2D< double > pose
Definition: simple_loop.cpp:74
legacy_pose2d.hpp
KobukiManager::kobuki
kobuki::Kobuki kobuki
Definition: initialisation.cpp:42
KobukiManager
Keyboard remote control for our robot core (mobile base).
Definition: initialisation.cpp:14
ecl::StandardException
ecl::Slot::connect
void connect(const std::string &topic)
kobuki::Parameters
Parameter list and validator for the kobuki.
Definition: parameters.hpp:42
kobuki.hpp
Device driver core interface.
KobukiManager::processStreamData
void processStreamData()
Definition: sigslots.cpp:50
KobukiManager::~KobukiManager
~KobukiManager()
Definition: simple_loop.cpp:43
getPose
ecl_geometry_PUBLIC Pose2D getPose(const Odom2D &odom)
kobuki::Kobuki
The core kobuki driver class.
Definition: kobuki.hpp:95
ecl::StandardException::what
const char * what() const
KobukiManager::KobukiManager
KobukiManager()
Default constructor, needs initialisation.
Definition: initialisation.cpp:18
time.hpp
KobukiManager::slot_stream_data
ecl::Slot slot_stream_data
Definition: sigslots.cpp:57
kobuki::Parameters::device_port
std::string device_port
The serial device port name [/dev/kobuki].
Definition: parameters.hpp:68
main
int main()
Definition: initialisation.cpp:43


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Wed Mar 2 2022 00:26:14