multi-robot systems with ros lesson 8
DESCRIPTION
Multi-Robot Systems with ROS Lesson 8. Teaching Assistant: Roi Yehoshua [email protected]. Agenda. Running navigation stack in Gazebo with multiple robots Sending goals to robots in Gazebo Using amcl Multi robot collision avoidance in navigation stack. Meet Lizi. - PowerPoint PPT PresentationTRANSCRIPT
![Page 2: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/2.jpg)
(C)2014 Roi Yehoshua
Agenda• TAO events• Allocating sub-plans• Defining custom Next protocols• Wandering robot example
![Page 3: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/3.jpg)
(C)2014 Roi Yehoshua
TAO Events• TAO defines an event distribution system using
an EventQueue• It’s used for sharing events inside TAO machines• It’s possible to insert external events to the
system (from ROS or other custom source)• It is thread safe
![Page 4: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/4.jpg)
(C)2014 Roi Yehoshua
TAO Event• Each Event is a path containing:– all context names– when this event was created– an event short name at the end
• Example: /ContextName/EventName • When you compare two events you can use
regular expressions as the name of one event by writing @ at the beggining of the name. – Example: @/Con.*/Event[NT]...e
![Page 5: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/5.jpg)
(C)2014 Roi Yehoshua
Events Inside TAO• TAO_RAISE(/STOP) - raise global event• TAO_RAISE(STOP) - raise an event related to TAO
context • TAO_RAISE(PLAN_NAME/STOP) - raise an event
related to TAO PLAN context event• TAO_STOP_CONDITION(event==TAO_EVENT(STOP))
- check condition based on an occurred event• TAO_EVENTS_DROP - Clear current events queue
![Page 6: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/6.jpg)
(C)2014 Roi Yehoshua
Events Outside TAO• Defining an event:– Event e("/STOP") - global / without context– Event e("STOP",call_context) - related to call_context
• Raising the event:– events->raiseEvent(e);
![Page 7: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/7.jpg)
(C)2014 Roi Yehoshua
Events Interface• Event waitEvent() - block function until a new
event arrives• Event tryGetEvent(bool& success) - get a new
event if exists (non-blocking call)• bool isTerminated() const - check if the event
system is closed• void drop_all() -
![Page 8: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/8.jpg)
(C)2014 Roi Yehoshua
Events InterfaceFunction
Block function until a new event arrives Event waitEvent )(Get a new event if exists (non-blocking call) Event tryGetEvent(bool&
success) Check if the event system is closed bool isTerminated)(Clear queue void drop_all)(Close event system, releasing all waiting processes void close)(
Send one /SPIN event for validation of STOP conditions
spinOne)(
Run spinOne() command with rate_in_hz spin(double rate_in_hz = 10)
Run spin command after start_delay without blocking current code execution
async_spin(double rate_in_hz = 10, double start_delay=0)
![Page 9: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/9.jpg)
(C)2014 Roi Yehoshua
RosEventQueue• A class derived from decision_making::EventQueue• RosEventQueue creates a connection between ROS
(/decision_making/NODE_NAME/events topic) and the internal EventQueue
• Must be created after ros::init and ros_decision_making_init
![Page 10: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/10.jpg)
(C)2014 Roi Yehoshua
Events Example• In the following example, we will add a support
for a STOP event to our Incrementer plan• Copy TaskWithStopCondition.cpp to Events.cpp• In the TAO machine definition, add a clause to
the STOP condition that checks if a STOP event has occurred
![Page 11: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/11.jpg)
(C)2014 Roi Yehoshua
TaoEvents.cppTAO(Incrementer){ TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(true); TAO_CALL_TASK(incrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(WM.counter == 100 || event == TAO_EVENT("/STOP")); TAO_NEXT_EMPTY } } TAO_END}
![Page 12: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/12.jpg)
(C)2014 Roi Yehoshua
Sending Events• Once running, your model will be able to receive
events over the decision_making/[NODE_NAME]/events topic
• Events to the model can be sent using:
• For example, to send a STOP event to the tao_events node, type:
$ rostopic pub decision_making/[NODE_NAME]/events std_msgs/String "EVENT_NAME"
rostopic pub decision_making/tao_events/events std_msgs/String "STOP"
![Page 13: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/13.jpg)
(C)2014 Roi Yehoshua
Sending Events Example
![Page 14: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/14.jpg)
(C)2014 Roi Yehoshua
Allocating SubPlans• We will now change the incrementer plan so it
allocates two sub-plans:– Even – this subplan will increment the counter when
it has an even value– Odd – this subplan will increment the counter when it
has an odd value• Copy BasicPlanWithWM.cpp to
AllocatingSubPlans.cpp
![Page 15: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/15.jpg)
(C)2014 Roi Yehoshua
AllocatingSubPlans.cpp (1)struct WorldModel : public CallContextParameters { int counter; bool even_plan_finished; bool odd_plan_finished; string str() const { stringstream s; s << "counter = " << counter; return s.str(); }};#define WM TAO_CONTEXT.parameters<WorldModel>()
![Page 16: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/16.jpg)
(C)2014 Roi Yehoshua
AllocatingSubPlans.cpp (2)TAO_HEADER(Even)TAO_HEADER(Odd) TAO(Incrementer){ TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(WM.counter < 100); WM.even_plan_finished = false; WM.odd_plan_finished = false; TAO_ALLOCATE(AllocFirstReady) { TAO_SUBPLAN(Even); TAO_SUBPLAN(Odd); } TAO_STOP_CONDITION(WM.even_plan_finished || WM.odd_plan_finished); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Increment); } } } TAO_END}
![Page 17: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/17.jpg)
(C)2014 Roi Yehoshua
AllocatingSubPlans.cpp (3)TAO(Even){ TAO_PLANS { Even, } TAO_START_PLAN(Even); TAO_BGN { TAO_PLAN(Even) { TAO_START_CONDITION(WM.counter % 2 == 0); WM.counter++; cout << "Even: " << WM.str() << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY WM.even_plan_finished = true; } } TAO_END}
![Page 18: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/18.jpg)
(C)2014 Roi Yehoshua
AllocatingSubPlans.cpp (4)TAO(Odd){ TAO_PLANS{ Odd, } TAO_START_PLAN(Odd); TAO_BGN { TAO_PLAN(Odd) { TAO_START_CONDITION(WM.counter % 2 == 1); WM.counter++; cout << "Odd: " << WM.str() << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY WM.odd_plan_finished = true; } } TAO_END}
![Page 19: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/19.jpg)
(C)2014 Roi Yehoshua
SubPlans Behavior• The sub-plans start executing only after the parent plan
reaches its stop condition (and not right after allocation)• If the parent’s stop condition is true before any of its
sub-plans started running, then the TAO machine ends• If the parent’s stop condition is false and the parent’s
plan has a next plan, then it is unpredictable whether the next plan or the sub-plan starts first
• Once a plan starts executing, no other plan can execute at the same time (unless using tasks)
• The entire TAO machine ends only when the parent plan and all its sub-plans are finished
![Page 20: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/20.jpg)
(C)2014 Roi Yehoshua
Allocating SubPlans Demo
![Page 21: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/21.jpg)
(C)2014 Roi Yehoshua
Decision Graph
![Page 22: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/22.jpg)
(C)2014 Roi Yehoshua
Wandering Robot Plan• Now we will write a TAO plan for a wandering robot• The wandering plan will be composed of two sub-
plans:– Drive – makes the robot drive forward until an obstacle is
detected– Turn – makes the robot turn until the way becomes clear
• Create a new package called tao_wandering:$ cd ~/dmw/src$ catkin_create_pkg tao_wandering roscpp decision_making decision_making_parser random_numbers sensor_msgs std_msgs geometry_msgs
![Page 23: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/23.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (1)#include <iostream> #include <ros/ros.h>#include <random_numbers/random_numbers.h>#include <sensor_msgs/LaserScan.h>#include <geometry_msgs/Twist.h> #include <decision_making/TAO.h>#include <decision_making/TAOStdProtocols.h>#include <decision_making/ROSTask.h>#include <decision_making/DecisionMaking.h> using namespace std;using namespace decision_making;
![Page 24: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/24.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (2)/*** Constants ***/#define MIN_SCAN_ANGLE_RAD -45.0/180*M_PI#define MAX_SCAN_ANGLE_RAD +45.0/180*M_PI#define MIN_DIST_TO_OBSTACLE 0.8 // in meters /*** Variables ***/random_numbers::RandomNumberGenerator _randomizer;ros::Publisher _velocityPublisher; /*** World model ***/struct WorldModel : public CallContextParameters { bool obstacleDetected; bool driveFinished; bool turnFinished; string str() const { stringstream s; s << "obstacleDetected = " << obstacleDetected; return s.str(); }};#define WM TAO_CONTEXT.parameters<WorldModel>()
![Page 25: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/25.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (3)/*** TAO machine ***/TAO_HEADER(Drive)TAO_HEADER(Turn)
TAO(Wandering){ TAO_PLANS { Wandering } TAO_START_PLAN(Wandering); TAO_BGN { TAO_PLAN(Wandering) { TAO_START_CONDITION(true); WM.driveFinished = false; WM.turnFinished = false; TAO_ALLOCATE(AllocFirstReady) { TAO_SUBPLAN(Drive); TAO_SUBPLAN(Turn); } TAO_STOP_CONDITION(WM.driveFinished || WM.turnFinished); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Wandering); } } } TAO_END}
![Page 26: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/26.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (4)TAO(Drive){ TAO_PLANS { Drive, } TAO_START_PLAN(Drive); TAO_BGN { TAO_PLAN(Drive) { TAO_START_CONDITION(!WM.obstacleDetected); TAO_CALL_TASK(driveTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(WM.obstacleDetected); TAO_NEXT_EMPTY WM.driveFinished = true; } } TAO_END}
![Page 27: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/27.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (5)TAO(Turn){ TAO_PLANS{ Turn, } TAO_START_PLAN(Turn); TAO_BGN { TAO_PLAN(Turn) { TAO_START_CONDITION(WM.obstacleDetected); TAO_CALL_TASK(turnTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY WM.turnFinished = true; } } TAO_END}
![Page 28: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/28.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (6)/*** Task implementations ***/TaskResult driveTask(string name, const CallContext& context, EventQueue& eventQueue) { ROS_INFO("Driving..."); geometry_msgs::Twist forwardMessage; forwardMessage.linear.x = 1.0; // Preemptive wait while (!eventQueue.isTerminated()) { _velocityPublisher.publish(forwardMessage); boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } ROS_INFO("Obstacle detected!"); return TaskResult::SUCCESS();}
![Page 29: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/29.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (7)TaskResult turnTask(string name, const CallContext& context, EventQueue& eventQueue) { ROS_INFO("Turning..."); bool turnLeft = _randomizer.uniformInteger(0, 1); geometry_msgs::Twist turnMessage; turnMessage.angular.z = 2 * (turnLeft ? 1 : -1); int timeToTurnMs = _randomizer.uniformInteger(2000, 4000); int turnLoops = timeToTurnMs / 100; for (int i = 0; i < turnLoops; i++) { _velocityPublisher.publish(turnMessage); boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } return TaskResult::SUCCESS();}
![Page 30: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/30.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (8)/*** ROS Subscriptions ***/void onLaserScanMessage(const sensor_msgs::LaserScan::Ptr laserScanMessage, CallContext* context) { bool obstacleFound = false; int minIndex = ceil((MIN_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment); int maxIndex = floor((MAX_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment); for (int i = minIndex; i <= maxIndex; i++) { if (laserScanMessage->ranges[i] < MIN_DIST_TO_OBSTACLE) { obstacleFound = true; } } context->parameters<WorldModel>().obstacleDetected = obstacleFound;}
![Page 31: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/31.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (9)int main(int argc, char **argv){ ros::init(argc, argv, "wandering_node"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); // ROS spinner for topic subscriptions ros::AsyncSpinner spinner(1); spinner.start(); // Tasks registration LocalTasks::registrate("driveTask", driveTask); LocalTasks::registrate("turnTask", turnTask); RosEventQueue eventQueue; CallContext context; context.createParameters(new WorldModel());
![Page 32: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/32.jpg)
(C)2014 Roi Yehoshua
Wandering.cpp (10) // CallContext must define a team teamwork::SharedMemory db; teamwork::Teammates teammates; teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates); context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr()); // Subscription for the laser topic and velocity publisher creation ros::Subscriber laserSubscriber = nh.subscribe<void>("base_scan", 1, boost::function<void(const sensor_msgs::LaserScan::Ptr)>(boost::bind(onLaserScanMessage, _1, &context))); _velocityPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100); eventQueue.async_spin(); ROS_INFO("Starting wandering machine..."); TaoWandering(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0;}
![Page 33: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/33.jpg)
(C)2014 Roi Yehoshua
Wandering Launch File• Copy worlds directory from
~/catkin_ws/src/gazebo_navigation_multi package• Create a launch directory in the tao_wandering
package• Add the following wandering.launch file
![Page 34: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/34.jpg)
(C)2014 Roi Yehoshua
Wandering Launch File
<launch> <master auto="start"/> <param name="/use_sim_time" value="true"/> <!-- start gazebo --> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(find tao_wandering)/worlds/willowgarage.world" /> </include> <param name="robot_description" command="$(find xacro)/xacro.py $(find lizi_description)/urdf/lizi.urdf"/> <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-x 34 -y 18 -z 0 -Y 1.57 -urdf -param robot_description -model lizi1" output="screen"/> <node name="wandering" pkg="tao_wandering" type="wandering_node" output="screen" />
</launch>
![Page 35: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/35.jpg)
(C)2014 Roi Yehoshua
Wandering Demo
![Page 36: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/36.jpg)
(C)2014 Roi Yehoshua
Custom Next Protocol• To define your own next protocol:– Create a class that inherits from
decision_making::ProtocolNext– Implement the pure virtual function decide()– Call setDecision at the end of the function with the
chosen plan ID
![Page 37: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/37.jpg)
(C)2014 Roi Yehoshua
Custom Next Protocol• In the following example, we will decompose the
Turn plan into TurnLeft and TurnRight subplans• We will create a custom RandomNext protocol
that will randomly choose the next plan from its set of candidate plans
• We will use this protocol to make the Turn plan randomly choose between TurnLeft and TurnRight as its continuation plan
![Page 38: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/38.jpg)
(C)2014 Roi Yehoshua
RandomNext Classclass NextRandom: public decision_making::ProtocolNext {public: NextRandom(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolNext(res, call_context, events){} bool decide(){ vector<int> ready_index; for(size_t i = 0; i < options.size(); i++) if (options[i].isReady) ready_index.push_back(i); if (ready_index.size() == 0) return false; int i = _randomizer.uniformInteger(0, ready_index.size() - 1); return setDecision(options[ready_index[i]].id); }};
![Page 39: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/39.jpg)
(C)2014 Roi Yehoshua
Turn Plan (1)TAO(Turn){ TAO_PLANS{ Turn, TurnLeft, TurnRight } TAO_START_PLAN(Turn); TAO_BGN { TAO_PLAN(Turn) { TAO_START_CONDITION(WM.obstacleDetected); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextRandom) { TAO_NEXT_PLAN(TurnLeft); TAO_NEXT_PLAN(TurnRight); } }
![Page 40: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/40.jpg)
(C)2014 Roi Yehoshua
Turn Plan (2) TAO_PLAN(TurnLeft) { TAO_START_CONDITION(true); TAO_ALLOCATE_EMPTY; WM.turnLeft = false; TAO_CALL_TASK(turnTask); TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY; WM.turnFinished = true; } TAO_PLAN(TurnRight) { TAO_START_CONDITION(true); TAO_ALLOCATE_EMPTY; WM.turnLeft = true; TAO_CALL_TASK(turnTask); TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY; WM.turnFinished = true; } } TAO_END}
![Page 41: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/41.jpg)
(C)2014 Roi Yehoshua
• Add a boolean variable to the world model indicating which turn direction was chosen:
World Model Changes
struct WorldModel : public CallContextParameters { bool obstacleDetected; bool driveFinished; bool turnFinished; bool turnLeft; string str() const { stringstream s; s << "obstacleDetected = " << obstacleDetected; return s.str(); }};#define WM TAO_CONTEXT.parameters<WorldModel>()
![Page 42: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/42.jpg)
(C)2014 Roi Yehoshua
• Make the following changes to the turnTask callback:
Turn Task
TaskResult turnTask(string name, const CallContext& context, EventQueue& eventQueue) { if (context.parameters<WorldModel>().turnLeft) ROS_INFO("Turning left..."); else ROS_INFO("Turning right..."); geometry_msgs::Twist turnMessage; turnMessage.angular.z = 2 * (context.parameters<WorldModel>().turnLeft ? 1 : -1); int timeToTurnMs = _randomizer.uniformInteger(2000, 4000); int turnLoops = timeToTurnMs / 100; for (int i = 0; i < turnLoops; i++) { _velocityPublisher.publish(turnMessage); boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } return TaskResult::SUCCESS();}
![Page 43: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/43.jpg)
(C)2014 Roi Yehoshua
Wandering Demo
![Page 44: Multi-Robot Systems with ROS Lesson 8](https://reader035.vdocument.in/reader035/viewer/2022081722/568163ae550346895dd4c31b/html5/thumbnails/44.jpg)
(C)2014 Roi Yehoshua
Homework (not for submission)• Create a custom next protocol for the turn plan that
will choose between the following 3 plans:– Turn random when there is an obstacle on the front– Turn left when there is an obstacle on the right side– Turn right when there is an obstacle on the left side
• Add a support for pausing/resuming the robot by sending an appropriate event