Wandering.cpp
Go to the documentation of this file.
00001 /*
00002  * Filename: Wandering.cpp
00003  *   Author: Igor Makhtes
00004  *     Date: Dec 16, 2013
00005  *
00006  * The MIT License (MIT)
00007  *
00008  * Copyright (c) 2013 Cogniteam Ltd.
00009  *
00010  * Permission is hereby granted, free of charge, to any person obtaining a copy
00011  * of this software and associated documentation files (the "Software"), to deal
00012  * in the Software without restriction, including without limitation the rights
00013  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00014  * copies of the Software, and to permit persons to whom the Software is
00015  * furnished to do so, subject to the following conditions:
00016  *
00017  * The above copyright notice and this permission notice shall be included in
00018  * all copies or substantial portions of the Software.
00019  *
00020  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00021  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00022  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00023  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00024  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00025  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00026  * THE SOFTWARE.
00027  */
00028 
00029 #include <iostream>
00030 
00031 #include <boost/bind.hpp>
00032 #include <boost/thread.hpp>
00033 #include <boost/foreach.hpp>
00034 
00035 #include <ros/ros.h>
00036 #include <random_numbers/random_numbers.h>
00037 #include <sensor_msgs/LaserScan.h>
00038 #include <geometry_msgs/Twist.h>
00039 
00040 #include <decision_making/FSM.h>
00041 #include <decision_making/ROSTask.h>
00042 #include <decision_making/DecisionMaking.h>
00043 
00044 using namespace std;
00045 using namespace decision_making;
00046 
00047 #define foreach BOOST_FOREACH
00048 
00049 /*************************************************************************************************
00050 *** Variables
00051 **************************************************************************************************/
00052 
00053 random_numbers::RandomNumberGenerator _randomizer;
00054 ros::Publisher _velocityPublisher;
00055 
00056 /*************************************************************************************************
00057 *** Final state machine
00058 **************************************************************************************************/
00059 
00060 FSM(Wandering)
00061 {
00062     FSM_STATES
00063     {
00064         Drive,
00065         Turn,
00066         Pause
00067     }
00068     FSM_START(Turn)
00069     FSM_BGN
00070     {
00071         FSM_STATE(Turn)
00072         {
00073             FSM_CALL_TASK(Turn)
00074 
00075             FSM_TRANSITIONS
00076             {
00077                 FSM_ON_EVENT("/TURN_TIMEOUT", FSM_NEXT(Drive))
00078                 FSM_ON_EVENT("/PAUSE", FSM_NEXT(Pause))
00079             }
00080         }
00081         FSM_STATE(Drive)
00082         {
00083             FSM_CALL_TASK(Drive)
00084 
00085             FSM_TRANSITIONS
00086             {
00087                 FSM_ON_EVENT("/DRIVE_TIMEOUT", FSM_NEXT(Turn))
00088                 FSM_ON_EVENT("/OBSTACLE", FSM_NEXT(Turn))
00089                 FSM_ON_EVENT("/PAUSE", FSM_NEXT(Pause))
00090             }
00091         }
00092         FSM_STATE(Pause)
00093         {
00094             FSM_CALL_TASK(Pause)
00095 
00096             FSM_TRANSITIONS
00097             {
00098                 FSM_ON_EVENT("/RESUME", FSM_NEXT(Turn))
00099             }
00100         }
00101     }
00102     FSM_END
00103 }
00104 
00105 
00106 /*************************************************************************************************
00107 *** ROS Subscriptions
00108 **************************************************************************************************/
00109 
00110 void onLaserScanMessage(const sensor_msgs::LaserScan::Ptr laserScanMessage, RosEventQueue* eventQueue) {
00111     double frontRange = laserScanMessage->ranges[laserScanMessage->ranges.size() / 2];
00112 
00113     if (frontRange < 0.5) {
00114         eventQueue->riseEvent("/OBSTACLE");
00115     }
00116 }
00117 
00118 
00119 /*************************************************************************************************
00120 *** Task implementations
00121 **************************************************************************************************/
00122 
00123 decision_making::TaskResult driveTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
00124     ROS_INFO("Driving...");
00125 
00126     double timeToDriveMs = _randomizer.uniformInteger(4000, 8000);
00127 
00128     geometry_msgs::Twist forwardMessage;
00129     forwardMessage.linear.x = 2;
00130     _velocityPublisher.publish(forwardMessage);
00131 
00135     for (int i = 0; i < 100; ++i) {
00136         if (eventQueue.isTerminated()) {
00137             ROS_INFO("Obstacle!");
00138             return TaskResult::TERMINATED();
00139         }
00140 
00141         boost::this_thread::sleep(boost::posix_time::milliseconds(timeToDriveMs / 100.0));
00142     }
00143 
00144     eventQueue.riseEvent("/DRIVE_TIMEOUT");
00145     return TaskResult::SUCCESS();
00146 }
00147 
00148 decision_making::TaskResult turnTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
00149     ROS_INFO("Turning...");
00150 
00151     bool turnRight = _randomizer.uniformInteger(0, 1);
00152     int timeToTurnMs = _randomizer.uniformInteger(2000, 4000);
00153 
00154     geometry_msgs::Twist turnMessage;
00155     turnMessage.angular.z = 2 * (turnRight ? 1 : -1);
00156     _velocityPublisher.publish(turnMessage);
00157 
00158     boost::this_thread::sleep(boost::posix_time::milliseconds(timeToTurnMs));
00159 
00160     eventQueue.riseEvent("/TURN_TIMEOUT");
00161     return decision_making::TaskResult::SUCCESS();
00162 }
00163 
00164 decision_making::TaskResult pauseTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
00165     ROS_INFO("Pausing...");
00166 
00167     geometry_msgs::Twist stopMessage;
00168     _velocityPublisher.publish(stopMessage);
00169 
00170     return decision_making::TaskResult::SUCCESS();
00171 }
00172 
00173 /*************************************************************************************************
00174 *** The Main
00175 **************************************************************************************************/
00176 
00177 int main(int argc, char **argv) {
00181     ros::init(argc, argv, "fsm_wandering");
00182     ros_decision_making_init(argc, argv);
00183     ros::NodeHandle nodeHandle("~");
00184     RosEventQueue eventQueue;
00185 
00189     LocalTasks::registrate("Drive", driveTask);
00190     LocalTasks::registrate("Turn", turnTask);
00191     LocalTasks::registrate("Pause", pauseTask);
00192 
00197     ros::Subscriber laserSubscriber = nodeHandle.subscribe<void>("scan_raw", 1,
00198                 boost::function<void(const sensor_msgs::LaserScan::Ptr)>(boost::bind(onLaserScanMessage, _1, &eventQueue)));
00199 
00200     _velocityPublisher = nodeHandle.advertise<geometry_msgs::Twist>("cmd_vel", 100, false);
00201 
00205     ros::AsyncSpinner spinner(1);
00206     spinner.start();
00207 
00211     ROS_INFO("Starting wandering machine...");
00212     FsmWandering(NULL, &eventQueue);
00213 
00217         return 0;
00218 }


decision_making_examples
Author(s):
autogenerated on Wed Aug 26 2015 11:17:01