ROS Lecture11

October 2016
ROS – Lecture 11
Behavior-Based Robotics
Where to go next?
Lecturer: Roi Yehoshua

roiyeho@gmail.com
Agenda
• Behavior-based robotics
• Decision making in ROS
• Where to go next?
(C)2016 Roi Yehoshua

• The world is fundamentally unknown and changing
• Does not make sense to over-plan
• Key idea: to develop a library of useful behaviors
(controllers)
• Switch among behaviors in response to
environmental changes

Behavior
• A behavior is a mapping of sensory inputs to a
pattern of motor actions
• Composed of:
– Start conditions (preconditions) - what must be true
to be executable
– Actions - what changes once behavior executes
– Stop conditions - when the behavior terminates

Behaviors
• Behaviors are independent and operate concurrently
– One behavior does not know what another behavior is
doing or perceiving
• The overall behavior of the robot is emergent
– There is no explicit “controller” module which determines
what will be done

Example: Navigation
• Problem: Go to a goal location without bumping
into obstacles
• Need at least two behaviors: Go-to-goal and

Avoid-obstacles

Behaviors
Plan changes
Identify objects
Monitor change
Map
Explore
Wander
Avoid object

Behavior-Based Example
• Create a new package gazebo_random_walk
$$ cd
cd ~/catkin_ws/src
~/catkin_ws/src
$$ catkin_create_pkg
catkin_create_pkg behavior_based
behavior_based std_msgs
std_msgs rospy
rospy roscpp
roscpp
• Create a launch subdirectory within the package and

add the following launch file to it

Behavior Class – Header File
#include
#include <vector>
<vector>
using
using namespace
namespace std;
std;

class
class Behavior
Behavior {{
private:
private:
vector<Behavior
vector<Behavior *> *> _nextBehaviors;
_nextBehaviors;

public:
public:
Behavior();
Behavior();
virtual
virtual bool
bool startCond()
startCond() == 0;
0;
virtual bool stopCond() =
virtual bool stopCond() = 0; 0;
virtual
virtual void
void action()
action() == 0;
0;

Behavior
Behavior *addNext(Behavior
*addNext(Behavior *beh);
*beh);
Behavior
Behavior *selectNext();
*selectNext();
virtual
virtual ~Behavior();
~Behavior();
};
};

Behavior Class – cpp File
#include
#include "Behavior.h"
"Behavior.h"
#include
#include <iostream>
<iostream>

Behavior::Behavior()
Behavior::Behavior() {{
}}

Behavior::~Behavior()
Behavior::~Behavior() {{
}}

Behavior
Behavior *Behavior::addNext(Behavior
*Behavior::addNext(Behavior *beh)
*beh) {{
_nextBehaviors.push_back(beh);
_nextBehaviors.push_back(beh);
return
return this;
this;
}}

Behavior
Behavior *Behavior::selectNext()
*Behavior::selectNext() {{
for
for (int
(int ii == 0;
0; ii << _nextBehaviors.size();
_nextBehaviors.size(); i++)
i++)
{{
if
if (_nextBehaviors[i]->startCond())
(_nextBehaviors[i]->startCond())
return
return _nextBehaviors[i];
_nextBehaviors[i];
}}
return
return NULL;
NULL;
}}

MoveForward Behavior - Header
#include
"Behavior.h"
#include <ros/ros.h>
#include
#include "sensor_msgs/LaserScan.h"
"sensor_msgs/LaserScan.h"
#include
#include <cmath>
<cmath>

class
class MoveForward:
MoveForward: public
public Behavior
Behavior {{
public:
public:
MoveForward();
MoveForward();
virtual
virtual bool
bool startCond();
startCond();
virtual void action();
virtual
virtual bool
bool stopCond();
stopCond();
virtual ~MoveForward();
virtual ~MoveForward();
private:
private:
const
const static
static double
double FORWARD_SPEED_MPS
FORWARD_SPEED_MPS == 0.5;
0.5;
const
const static
static double
double MIN_SCAN_ANGLE_RAD
MIN_SCAN_ANGLE_RAD == -30.0/180
-30.0/180 ** M_PI;
M_PI;
const
const static
static double
double MAX_SCAN_ANGLE_RAD
MAX_SCAN_ANGLE_RAD == +30.0/180
+30.0/180 ** M_PI;
M_PI;
const static float MIN_PROXIMITY_RANGE_M = 0.5;

ros::NodeHandle
ros::NodeHandle node;
node;
ros::Publisher commandPub;
ros::Subscriber
ros::Subscriber laserSub;
laserSub;

void
void scanCallback(const
scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan);
scan);
bool
bool keepMovingForward;
keepMovingForward;
};
};

MoveForward Behavior – cpp (1)
#include
#include "MoveForward.h"
"MoveForward.h"
#include
#include "geometry_msgs/Twist.h"
"geometry_msgs/Twist.h"

MoveForward::MoveForward()
MoveForward::MoveForward() {{
commandPub
commandPub == node.advertise<geometry_msgs::Twist>("cmd_vel",
node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
10);
laserSub
laserSub = node.subscribe("base_scan", 1, &MoveForward::scanCallback, this);
= node.subscribe("base_scan", 1, &MoveForward::scanCallback, this);
keepMovingForward
keepMovingForward == true;
true;
}}

bool
bool MoveForward::startCond()
MoveForward::startCond() {{
return
return keepMovingForward;
keepMovingForward;
}}

void
void MoveForward::action()
MoveForward::action() {{
geometry_msgs::Twist
geometry_msgs::Twist msg;
msg;
msg.linear.x = FORWARD_SPEED_MPS;
msg.linear.x = FORWARD_SPEED_MPS;
commandPub.publish(msg);
ROS_INFO("Moving
ROS_INFO("Moving forward");
forward");
}}

bool
bool MoveForward::stopCond()
MoveForward::stopCond() {{
return
return !keepMovingForward;
!keepMovingForward;
}}

MoveForward Behavior – cpp (2)
void
void MoveForward::scanCallback(const
MoveForward::scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan)
scan)
{{
//
// Find
Find the
the closest
closest range
range between
between the
the defined
defined minimum
minimum and
and maximum
maximum angles
angles
int
int minIndex
minIndex == ceil((MIN_SCAN_ANGLE_RAD
ceil((MIN_SCAN_ANGLE_RAD -- scan->angle_min)
scan->angle_min) // scan->angle_increment);
scan->angle_increment);
int
int maxIndex
maxIndex == floor((MAX_SCAN_ANGLE_RAD
floor((MAX_SCAN_ANGLE_RAD -- scan->angle_min)
scan->angle_min) // scan-
scan-
>angle_increment);
>angle_increment);

float
float closestRange
closestRange == scan->ranges[minIndex];
scan->ranges[minIndex];
for
for (int currIndex = minIndex ++ 1;
(int currIndex = minIndex 1; currIndex
currIndex <=
<= maxIndex;
maxIndex; currIndex++)
currIndex++) {{
if
if (scan->ranges[currIndex]
(scan->ranges[currIndex] << closestRange)
closestRange) {{
closestRange
closestRange == scan->ranges[currIndex];
scan->ranges[currIndex];
}}
}}

if
if (closestRange
(closestRange << MIN_PROXIMITY_RANGE_M)
MIN_PROXIMITY_RANGE_M) {{
keepMovingForward
keepMovingForward == false;
false;
}} else
else {{
keepMovingForward
keepMovingForward == true;
true;
}}
}}

MoveForward::~MoveForward()
MoveForward::~MoveForward() { {
}}

TurnLeft Behavior - Header
#include
"Behavior.h"
#include
#include "sensor_msgs/LaserScan.h"
"sensor_msgs/LaserScan.h"
#include
#include <cmath>
<cmath>

class
class TurnLeft:
TurnLeft: public
public Behavior
Behavior {{
public:
public:
TurnLeft();
TurnLeft();
virtual
virtual bool
bool startCond();
startCond();
virtual
virtual bool
bool stopCond();
stopCond();
virtual ~TurnLeft();
virtual ~TurnLeft();
private:
private:
const
const static
static double
double TURN_SPEED_MPS
TURN_SPEED_MPS == 1.0;
1.0;
const
const static
static double
double MIN_SCAN_ANGLE_RAD
MIN_SCAN_ANGLE_RAD == -30.0/180
-30.0/180 ** M_PI;
M_PI;
const
const static
static double
double MAX_SCAN_ANGLE_RAD
MAX_SCAN_ANGLE_RAD == 0;
0;
const
static float MIN_PROXIMITY_RANGE_M = 0.5;

ros::NodeHandle
ros::NodeHandle node;
node;
ros::Subscriber
ros::Subscriber laserSub;
laserSub;

void
void scanCallback(const
scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan);
scan);
bool
bool keepTurningLeft;
keepTurningLeft;
};
};

TurnLeft Behavior – cpp (1)
#include
#include "TurnLeft.h"
"TurnLeft.h"
#include
#include "geometry_msgs/Twist.h"
"geometry_msgs/Twist.h"

TurnLeft::TurnLeft()
TurnLeft::TurnLeft() {{
commandPub
commandPub == node.advertise<geometry_msgs::Twist>("cmd_vel",
node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
10);
laserSub
laserSub = node.subscribe("base_scan", 1, &TurnLeft::scanCallback, this);
= node.subscribe("base_scan", 1, &TurnLeft::scanCallback, this);
keepTurningLeft
keepTurningLeft == true;
true;
}}

bool
bool TurnLeft::startCond()
TurnLeft::startCond() {{
return
return keepTurningLeft;
keepTurningLeft;
}}

void
void TurnLeft::action()
TurnLeft::action() {{
geometry_msgs::Twist
geometry_msgs::Twist msg;
msg;
msg.angular.z = TURN_SPEED_MPS;
msg.angular.z = TURN_SPEED_MPS;
ROS_INFO("Turning
ROS_INFO("Turning left");
left");
}}

bool
bool TurnLeft::stopCond()
TurnLeft::stopCond() {{
return
return !keepTurningLeft;
!keepTurningLeft;
}}

TurnLeft Behavior – cpp (2)
void
void TurnLeft::scanCallback(const
TurnLeft::scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan)
scan)
{{
//
// Find
Find the
the closest
closest range
range between
between the
the defined
defined minimum
minimum and
and maximum
maximum angles
angles
int
int minIndex
minIndex == ceil((MIN_SCAN_ANGLE_RAD
ceil((MIN_SCAN_ANGLE_RAD -- scan->angle_min)
scan->angle_min) // scan->angle_increment);
scan->angle_increment);
int
int maxIndex
maxIndex == floor((MAX_SCAN_ANGLE_RAD
floor((MAX_SCAN_ANGLE_RAD -- scan->angle_min)
scan->angle_min) // scan-
scan-
>angle_increment);
>angle_increment);

float
float closestRange
closestRange == scan->ranges[minIndex];
scan->ranges[minIndex];
for
for (int currIndex = minIndex ++ 1;
(int currIndex = minIndex 1; currIndex
currIndex <=
<= maxIndex;
maxIndex; currIndex++)
currIndex++) {{
if
if (scan->ranges[currIndex]
(scan->ranges[currIndex] << closestRange)
closestRange) {{
closestRange
closestRange == scan->ranges[currIndex];
scan->ranges[currIndex];
}}
}}

if
if (closestRange
(closestRange << MIN_PROXIMITY_RANGE_M)
MIN_PROXIMITY_RANGE_M) {{
keepTurningLeft
keepTurningLeft == true;
true;
}} else
else {{
keepTurningLeft
keepTurningLeft == false;
false;
}}
}}

TurnLeft::~TurnLeft()
TurnLeft::~TurnLeft() { {
}}

Two main challenges in behavior-based robotics
• Behavior selection - How do we select the
correct behavior?
• Behavior fusion – if several behaviors run in
parallel:
– How to merge the results?
– How to determine weight of each behavior in the
result?

State-Based Selection
C
A A
C
B C
Start
A D
A
• Every state represents a behavior

• Transitions are triggered by sensor readings

Example: Robot Soccer

Plan Class – Header file
#include
#include <vector>
<vector>
#include
"Behavior.h"
using
using namespace
namespace std;
std;

class
class Plan
Plan {{
public:
public:
Plan();
Plan();
Behavior
Behavior *getStartBehavior();
*getStartBehavior();
virtual
virtual ~Plan();
~Plan();
protected:
protected:
vector<Behavior
vector<Behavior *>*> behaviors;
behaviors;
Behavior
Behavior *startBehavior;
*startBehavior;
};
};

Plan Class – cpp File
#include
#include "Plan.h"
"Plan.h"
#include
#include <iostream>
<iostream>

Plan::Plan()
Plan::Plan() :: startBehavior(NULL)
startBehavior(NULL) {{

}}

Behavior
Behavior *Plan::getStartBehavior()
*Plan::getStartBehavior() {{
return
return startBehavior;
startBehavior;
}}

Plan::~Plan()
Plan::~Plan() {{

}}

ObstacleAvoidPlan – Header File
#include
#include "Plan.h"
"Plan.h"

class
class ObstacleAvoidPlan:
ObstacleAvoidPlan: public
public Plan
Plan {{
public:
public:
ObstacleAvoidPlan();
ObstacleAvoidPlan();
virtual
virtual ~ObstacleAvoidPlan();
~ObstacleAvoidPlan();
};
};

ObstacleAvoidPlan – cpp File
#include
#include "ObstacleAvoidPlan.h"
"ObstacleAvoidPlan.h"
#include
#include "MoveForward.h"
"MoveForward.h"
#include
#include "TurnLeft.h"
"TurnLeft.h"
#include
#include "TurnRight.h"
"TurnRight.h"

ObstacleAvoidPlan::ObstacleAvoidPlan()
ObstacleAvoidPlan::ObstacleAvoidPlan() {{
//
// Creating
Creating behaviors
behaviors
behaviors.push_back(new
behaviors.push_back(new MoveForward());
MoveForward());
behaviors.push_back(new TurnLeft());
TurnLeft());
behaviors.push_back(new TurnRight());
TurnRight());

//
// Connecting
Connecting behaviors
behaviors
behaviors[0]->addNext(behaviors[1]);


startBehavior
startBehavior == behaviors[0];
behaviors[0];
}}

Manager – Header File
#include
#include "Plan.h"
"Plan.h"
#include
"Behavior.h"

class
class Manager
Manager {{
public:
public:
Manager(Plan
Manager(Plan *plan);
*plan);
void
void run();
run();
virtual
virtual ~Manager();
~Manager();
private:
private:
Plan
Plan *plan;
*plan;
Behavior
Behavior *currBehavior;
*currBehavior;
};
};

Manager – cpp File
#include
#include "Manager.h"
"Manager.h"
#include
<ros/ros.h>

Manager::Manager(Plan
Manager::Manager(Plan *plan)
*plan) :: plan(plan)
plan(plan) {{
currBehavior
currBehavior == plan->getStartBehavior();
plan->getStartBehavior();
}}

void
void Manager::run()
Manager::run()
{{
ros::Rate
ros::Rate rate(10);
rate(10);
if
if (!currBehavior->startCond())
(!currBehavior->startCond()) {{
ROS_ERROR("Cannot
ROS_ERROR("Cannot start
start the
the first
first behavior");
behavior");
return;
return;
}
}
while
while (ros::ok()
(ros::ok() &&
&& currBehavior
currBehavior !=
!= NULL)
NULL) {{
currBehavior->action();
currBehavior->action();

if
if (currBehavior->stopCond())
(currBehavior->stopCond()) {{
currBehavior
currBehavior == currBehavior->selectNext();
currBehavior->selectNext();
}}
ros::spinOnce();
ros::spinOnce();
rate.sleep();
rate.sleep();
}}
ROS_INFO("Manager
ROS_INFO("Manager stopped");
stopped");
}}

Run.cpp
#include
#include "Manager.h"
"Manager.h"
#include
#include "ObstacleAvoidPlan.h"
"ObstacleAvoidPlan.h"

int
int main(int
main(int argc,
argc, char
char **argv)
**argv) {{
ros::init(argc,
ros::init(argc, argv,
argv, "behavior_based_wanderer");
"behavior_based_wanderer");

ObstacleAvoidPlan
ObstacleAvoidPlan plan;
plan;
Manager manager(&plan);
Manager manager(&plan);

//
// Start
Start the
the movement
movement
manager.run();
manager.run();

return
return 0;
0;
};
};

• Create a launch subdirectory within the package and
copy the launch file from gazebo_random_walk package
• Change the following lines in the launch file

behavior_based_wanderer.launch
<launch>
<launch>
<param
<param name="/use_sim_time"
name="/use_sim_time" value="true"
value="true" />
/>

-->
<include
<include file="$(find
file="$(find gazebo_ros)/launch/willowgarage_world.launch"/>
gazebo_ros)/launch/willowgarage_world.launch"/>
<arg
<arg name="init_pose"
name="init_pose" value="-x
value="-x -5
-5 -y
-y -2
-2 -z
-z 1"/>
1"/>
<param
<param name="robot_description"
name="robot_description" textfile="$(find
textfile="$(find r2d2_description)/urdf/r2d2.urdf"/>
r2d2_description)/urdf/r2d2.urdf"/>

-->
<node
<node name="spawn_urdf"
name="spawn_urdf" pkg="gazebo_ros"
pkg="gazebo_ros" type="spawn_model"
type="spawn_model" args="$(arg
args="$(arg init_pose)
init_pose) -urdf
-urdf
-param
-param robot_description
robot_description -model
-model my_robot"
my_robot" output="screen"/>
output="screen"/>
<node
<node name="joint_state_publisher"
name="joint_state_publisher" pkg="joint_state_publisher"
pkg="joint_state_publisher" type="joint_state_publisher"/>
type="joint_state_publisher"/>
<node
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
output="screen"/>
output="screen"/>

-->
<node
<node name="behavior_based_wanderer"
name="behavior_based_wanderer" pkg="behavior_based"
pkg="behavior_based" type="behavior_based_wanderer"
type="behavior_based_wanderer"
output="screen"/>
output="screen"/>
</launch>
</launch>


Decision Making in ROS
• http://wiki.ros.org/decision_making
• Light-weight, generic and extendable tools for
writing, executing, debugging and monitoring
decision making models through ROS standard tools
• Supports different decision making models:
– Finite State Machines
– Hierarchical FSMs
– Behavior Trees
– BDI
• Developed by Cogniteam
Where To Go Next?
• There are still many areas of ROS to explore:
– 3-D image processing using PCL
– Identifying your friends and family using
face_recognition
– Identifying and grasping objects on a table top
• or how about playing chess?
– Building knowledge bases with knowrob
– Learning from experience using
reinforcement_learning

Where To Go Next?
• There are now over 2000 packages and libraries
available for ROS
• Click on the Browse Software link at the top of
the ROS Wiki for a list of all ROS packages and
stacks that have been submitted for indexing.
• When you are ready, you can contribute your
own package(s) back to the ROS community
• Welcome to the future of robotics.
• Have fun and good luck!

ROS Lecture11

Caricato da

Informazioni sul documento

Titolo originale

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

ROS Lecture11

Caricato da

Copyright:

Formati disponibili

October 2016

Lecturer: Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

• Need at least two behaviors: Go-to-goal and

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

• Create a launch subdirectory within the package and

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

• Every state represents a behavior

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

(C)2016 Roi Yehoshua

Potrebbero piacerti anche