Public Member Functions | Private Member Functions | Private Attributes | List of all members
RobotOperator Class Reference

#include <RobotOperator.h>

Public Member Functions

void executeCommand ()
 Generates and sends Twist-Message to Robot This is the Operator's core function and should be called periodically. More...
 
void receiveCommand (const nav2d_operator::cmd::ConstPtr &msg)
 Callback function to receive move commands. More...
 
 RobotOperator ()
 
 ~RobotOperator ()
 

Private Member Functions

int calculateFreeSpace (sensor_msgs::PointCloud *cloud)
 Calculates the distance the robot can move following the given trajectory. More...
 
double evaluateAction (double direction, double velocity, bool debug=false)
 Calculate the action value of a given command. More...
 
double findBestDirection ()
 Evaluates all possible directions with a fixed resolution. More...
 
sensor_msgs::PointCloud * getPointCloud (double direction, double velocity)
 Get the trajectory defined by the given movement command. More...
 
void initTrajTable ()
 Initializes look-up tables This calculates the trajectories of all possible direction commands. Must be called once before the Operator is used. More...
 

Private Attributes

ros::Subscriber mCommandSubscriber
 
int mConformanceWeight
 
int mContinueWeight
 
ros::Publisher mControlPublisher
 
costmap_2d::Costmap2DmCostmap
 
ros::Publisher mCostPublisher
 
double mCurrentDirection
 
double mCurrentVelocity
 
double mDesiredDirection
 
double mDesiredVelocity
 
int mDriveMode
 
int mEscapeWeight
 
costmap_2d::Costmap2DROSmLocalMap
 
double mMaxFreeSpace
 
double mMaxVelocity
 
std::string mOdometryFrame
 
ros::Publisher mPlanPublisher
 
bool mPublishRoute
 
double mRasterSize
 
unsigned int mRecoverySteps
 
std::string mRobotFrame
 
double mSafetyDecay
 
int mSafetyWeight
 
tf2_ros::Buffer mTf2Buffer
 
tf2_ros::TransformListener mTf2Listener
 
tf::TransformListener mTfListener
 
ros::Publisher mTrajectoryPublisher
 
sensor_msgs::PointCloud * mTrajTable [(LUT_RESOLUTION *4)+2]
 

Detailed Description

Author
Sebastian Kasperski
Date
03/25/14

Definition at line 32 of file RobotOperator.h.

Constructor & Destructor Documentation

◆ RobotOperator()

RobotOperator::RobotOperator ( )

Definition at line 8 of file RobotOperator.cpp.

◆ ~RobotOperator()

RobotOperator::~RobotOperator ( )

Definition at line 57 of file RobotOperator.cpp.

Member Function Documentation

◆ calculateFreeSpace()

int RobotOperator::calculateFreeSpace ( sensor_msgs::PointCloud *  cloud)
private

Calculates the distance the robot can move following the given trajectory.

Parameters
cloudPointCloud defining a trajectory
Returns
Nmber of free cells

Definition at line 364 of file RobotOperator.cpp.

◆ evaluateAction()

double RobotOperator::evaluateAction ( double  direction,
double  velocity,
bool  debug = false 
)
private

Calculate the action value of a given command.

Parameters
directionHow to move the robot
velocityOnly used to distinguish forward and backward motion
debugPublish result of evaluation functions on debug topic
Returns
Weighted sum of all evaluation functions The given action is rated by 4 different evaluation functions: Free Space: How far can the robot move following this command Safety: How close will it get near obstacles Conformance: How good does it follow the commanded direction Continuity: (experimental!) How does it conform with the last issued command

Definition at line 388 of file RobotOperator.cpp.

◆ executeCommand()

void RobotOperator::executeCommand ( )

Generates and sends Twist-Message to Robot This is the Operator's core function and should be called periodically.

Definition at line 200 of file RobotOperator.cpp.

◆ findBestDirection()

double RobotOperator::findBestDirection ( )
private

Evaluates all possible directions with a fixed resolution.

Returns
Best evaluated direction

Definition at line 489 of file RobotOperator.cpp.

◆ getPointCloud()

sensor_msgs::PointCloud * RobotOperator::getPointCloud ( double  direction,
double  velocity 
)
inlineprivate

Get the trajectory defined by the given movement command.

Parameters
directionHow to move the robot
velocityOnly used to distinguish forward and backward motion
Returns
A pointer to the PointCloud defined in the robot coordinate frame

Definition at line 509 of file RobotOperator.cpp.

◆ initTrajTable()

void RobotOperator::initTrajTable ( )
private

Initializes look-up tables This calculates the trajectories of all possible direction commands. Must be called once before the Operator is used.

Definition at line 65 of file RobotOperator.cpp.

◆ receiveCommand()

void RobotOperator::receiveCommand ( const nav2d_operator::cmd::ConstPtr &  msg)

Callback function to receive move commands.

Parameters
msgCommand-Message Direction [-1.0 .. 1.0]: -1(rotate left); 0(straight); 1(rotate right) Velocity [-1.0 .. 1.0]: -1(full speed back); 0(stop); 1(full speed ahead) Mode: 0(Avoid obstacles); 1(Stop at obstacles)

Definition at line 182 of file RobotOperator.cpp.

Member Data Documentation

◆ mCommandSubscriber

ros::Subscriber RobotOperator::mCommandSubscriber
private

Definition at line 108 of file RobotOperator.h.

◆ mConformanceWeight

int RobotOperator::mConformanceWeight
private

Definition at line 128 of file RobotOperator.h.

◆ mContinueWeight

int RobotOperator::mContinueWeight
private

Definition at line 129 of file RobotOperator.h.

◆ mControlPublisher

ros::Publisher RobotOperator::mControlPublisher
private

Definition at line 109 of file RobotOperator.h.

◆ mCostmap

costmap_2d::Costmap2D* RobotOperator::mCostmap
private

Definition at line 101 of file RobotOperator.h.

◆ mCostPublisher

ros::Publisher RobotOperator::mCostPublisher
private

Definition at line 112 of file RobotOperator.h.

◆ mCurrentDirection

double RobotOperator::mCurrentDirection
private

Definition at line 117 of file RobotOperator.h.

◆ mCurrentVelocity

double RobotOperator::mCurrentVelocity
private

Definition at line 116 of file RobotOperator.h.

◆ mDesiredDirection

double RobotOperator::mDesiredDirection
private

Definition at line 115 of file RobotOperator.h.

◆ mDesiredVelocity

double RobotOperator::mDesiredVelocity
private

Definition at line 114 of file RobotOperator.h.

◆ mDriveMode

int RobotOperator::mDriveMode
private

Definition at line 118 of file RobotOperator.h.

◆ mEscapeWeight

int RobotOperator::mEscapeWeight
private

Definition at line 130 of file RobotOperator.h.

◆ mLocalMap

costmap_2d::Costmap2DROS* RobotOperator::mLocalMap
private

Definition at line 100 of file RobotOperator.h.

◆ mMaxFreeSpace

double RobotOperator::mMaxFreeSpace
private

Definition at line 125 of file RobotOperator.h.

◆ mMaxVelocity

double RobotOperator::mMaxVelocity
private

Definition at line 122 of file RobotOperator.h.

◆ mOdometryFrame

std::string RobotOperator::mOdometryFrame
private

Definition at line 132 of file RobotOperator.h.

◆ mPlanPublisher

ros::Publisher RobotOperator::mPlanPublisher
private

Definition at line 111 of file RobotOperator.h.

◆ mPublishRoute

bool RobotOperator::mPublishRoute
private

Definition at line 124 of file RobotOperator.h.

◆ mRasterSize

double RobotOperator::mRasterSize
private

Definition at line 102 of file RobotOperator.h.

◆ mRecoverySteps

unsigned int RobotOperator::mRecoverySteps
private

Definition at line 135 of file RobotOperator.h.

◆ mRobotFrame

std::string RobotOperator::mRobotFrame
private

Definition at line 133 of file RobotOperator.h.

◆ mSafetyDecay

double RobotOperator::mSafetyDecay
private

Definition at line 126 of file RobotOperator.h.

◆ mSafetyWeight

int RobotOperator::mSafetyWeight
private

Definition at line 127 of file RobotOperator.h.

◆ mTf2Buffer

tf2_ros::Buffer RobotOperator::mTf2Buffer
private

Definition at line 105 of file RobotOperator.h.

◆ mTf2Listener

tf2_ros::TransformListener RobotOperator::mTf2Listener
private

Definition at line 106 of file RobotOperator.h.

◆ mTfListener

tf::TransformListener RobotOperator::mTfListener
private

Definition at line 104 of file RobotOperator.h.

◆ mTrajectoryPublisher

ros::Publisher RobotOperator::mTrajectoryPublisher
private

Definition at line 110 of file RobotOperator.h.

◆ mTrajTable

sensor_msgs::PointCloud* RobotOperator::mTrajTable[(LUT_RESOLUTION *4)+2]
private

Definition at line 120 of file RobotOperator.h.


The documentation for this class was generated from the following files:


nav2d_operator
Author(s): Sebastian Kasperski
autogenerated on Mon Feb 28 2022 22:56:59