1 / 44

Multi-Robot Systems with ROS Lesson 10

Multi-Robot Systems with ROS Lesson 10. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com. Agenda. TAO events Allocating sub-plans Defining custom Next protocols Wandering robot example. TAO Events. TAO defines an event distribution system using an EventQueue

joshua
Download Presentation

Multi-Robot Systems with ROS Lesson 10

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Multi-Robot Systems with ROS Lesson 10 Teaching Assistant: RoiYehoshua roiyeho@gmail.com

  2. Agenda • TAO events • Allocating sub-plans • Defining custom Next protocols • Wandering robot example (C)2014 Roi Yehoshua

  3. 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 (C)2014 Roi Yehoshua

  4. 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 (C)2014 Roi Yehoshua

  5. 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 (C)2014 Roi Yehoshua

  6. 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); (C)2014 Roi Yehoshua

  7. Events Interface • Event waitEvent() - block function until a new event arrives • Event tryGetEvent(bool& success) - get a new event if exists (non-blocking call) • boolisTerminated() const - check if the event system is closed • void drop_all() - (C)2014 Roi Yehoshua

  8. Events Interface (C)2014 Roi Yehoshua

  9. 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 (C)2014 Roi Yehoshua

  10. 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 (C)2014 Roi Yehoshua

  11. TaoEvents.cpp TAO(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 } (C)2014 Roi Yehoshua

  12. 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" (C)2014 Roi Yehoshua

  13. Sending Events Example (C)2014 Roi Yehoshua

  14. 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 (C)2014 Roi Yehoshua

  15. AllocatingSubPlans.cpp (1) structWorldModel : publicCallContextParameters { int counter; booleven_plan_finished; boolodd_plan_finished; string str() const { stringstream s; s << "counter = " << counter; return s.str(); } }; #define WM TAO_CONTEXT.parameters<WorldModel>() (C)2014 Roi Yehoshua

  16. 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 } (C)2014 Roi Yehoshua

  17. 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 } (C)2014 Roi Yehoshua

  18. 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 } (C)2014 Roi Yehoshua

  19. 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 (C)2014 Roi Yehoshua

  20. Allocating SubPlans Demo (C)2014 Roi Yehoshua

  21. Decision Graph (C)2014 Roi Yehoshua

  22. 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_pkgtao_wanderingroscppdecision_makingdecision_making_parserrandom_numberssensor_msgsstd_msgsgeometry_msgs (C)2014 Roi Yehoshua

  23. 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; (C)2014 Roi Yehoshua

  24. 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 ***/ structWorldModel : publicCallContextParameters { boolobstacleDetected; booldriveFinished; boolturnFinished; string str() const { stringstream s; s << "obstacleDetected = " << obstacleDetected; return s.str(); } }; #define WM TAO_CONTEXT.parameters<WorldModel>() (C)2014 Roi Yehoshua

  25. 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 } (C)2014 Roi Yehoshua

  26. 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 } (C)2014 Roi Yehoshua

  27. 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 } (C)2014 Roi Yehoshua

  28. Wandering.cpp (6) /*** Task implementations***/ TaskResultdriveTask(string name, constCallContext& 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!"); returnTaskResult::SUCCESS(); } (C)2014 Roi Yehoshua

  29. Wandering.cpp (7) TaskResultturnTask(string name, constCallContext& context, EventQueue& eventQueue) { ROS_INFO("Turning..."); boolturnLeft = _randomizer.uniformInteger(0, 1); geometry_msgs::Twist turnMessage; turnMessage.angular.z = 2 * (turnLeft ? 1 : -1); inttimeToTurnMs = _randomizer.uniformInteger(2000, 4000); intturnLoops = timeToTurnMs / 100; for (inti = 0; i < turnLoops; i++) { _velocityPublisher.publish(turnMessage); boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } returnTaskResult::SUCCESS(); } (C)2014 Roi Yehoshua

  30. Wandering.cpp (8) /***ROS Subscriptions***/ voidonLaserScanMessage(constsensor_msgs::LaserScan::PtrlaserScanMessage, CallContext* context) { boolobstacleFound = false; intminIndex = ceil((MIN_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment); intmaxIndex = floor((MAX_SCAN_ANGLE_RAD - laserScanMessage->angle_min) / laserScanMessage->angle_increment); for (inti = minIndex; i <= maxIndex; i++) { if (laserScanMessage->ranges[i] < MIN_DIST_TO_OBSTACLE) { obstacleFound = true; } } context->parameters<WorldModel>().obstacleDetected = obstacleFound; } (C)2014 Roi Yehoshua

  31. Wandering.cpp (9) int main(intargc, char **argv) { ros::init(argc, argv, "wandering_node"); ros::NodeHandlenh; 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); RosEventQueueeventQueue; CallContext context; context.createParameters(newWorldModel()); (C)2014 Roi Yehoshua

  32. 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(constsensor_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; } (C)2014 Roi Yehoshua

  33. 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 (C)2014 Roi Yehoshua

  34. 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 -paramrobot_description -model lizi1" output="screen"/> <node name="wandering" pkg="tao_wandering" type="wandering_node" output="screen" /> </launch> (C)2014 Roi Yehoshua

  35. Wandering Demo (C)2014 Roi Yehoshua

  36. 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 (C)2014 Roi Yehoshua

  37. Custom Next Protocol • In the following example, we will decompose the Turn plan into TurnLeft and TurnRightsubplans • 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 (C)2014 Roi Yehoshua

  38. RandomNext Class classNextRandom: publicdecision_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_ti = 0; i < options.size(); i++) if (options[i].isReady) ready_index.push_back(i); if (ready_index.size() == 0) returnfalse; inti = _randomizer.uniformInteger(0, ready_index.size() - 1); returnsetDecision(options[ready_index[i]].id); } }; (C)2014 Roi Yehoshua

  39. 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); } } (C)2014 Roi Yehoshua

  40. 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 } (C)2014 Roi Yehoshua

  41. World Model Changes • Add a boolean variable to the world model indicating which turn direction was chosen: structWorldModel : publicCallContextParameters { boolobstacleDetected; booldriveFinished; boolturnFinished; boolturnLeft; string str() const { stringstream s; s << "obstacleDetected = " << obstacleDetected; return s.str(); } }; #define WM TAO_CONTEXT.parameters<WorldModel>() (C)2014 Roi Yehoshua

  42. Turn Task • Make the following changes to the turnTask callback: TaskResultturnTask(string name, constCallContext& 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); inttimeToTurnMs = _randomizer.uniformInteger(2000, 4000); intturnLoops = timeToTurnMs / 100; for (inti = 0; i < turnLoops; i++) { _velocityPublisher.publish(turnMessage); boost::this_thread::sleep(boost::posix_time::milliseconds(100.0)); } returnTaskResult::SUCCESS(); } (C)2014 Roi Yehoshua

  43. Wandering Demo (C)2014 Roi Yehoshua

  44. 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 (C)2014 Roi Yehoshua

More Related