Represents one robot in STDR. Inherts publicly from nodelet::Nodelet. More...
#include <stdr_robot.h>
Public Member Functions | |
void | initializeRobot (const actionlib::SimpleClientGoalState &state, const stdr_msgs::RegisterRobotResultConstPtr result) |
Initializes the robot after on registering it to server. More... | |
void | mapCallback (const nav_msgs::OccupancyGridConstPtr &msg) |
Callback for getting the occupancy grid map. More... | |
bool | moveRobotCallback (stdr_msgs::MoveRobot::Request &req, stdr_msgs::MoveRobot::Response &res) |
The callback of the re-place robot service. More... | |
void | onInit (void) |
Initializes the robot and gets the environment occupancy grid map. More... | |
Robot (void) | |
Default constructor. More... | |
~Robot (void) | |
Default destructor. More... | |
Public Member Functions inherited from nodelet::Nodelet | |
void | init (const std::string &name, const M_string &remapping_args, const V_string &my_argv, ros::CallbackQueueInterface *st_queue=NULL, ros::CallbackQueueInterface *mt_queue=NULL) |
Nodelet () | |
virtual | ~Nodelet () |
Private Member Functions | |
bool | checkUnknownOccupancy (const geometry_msgs::Pose2D &newPose) |
Checks the robot's reposition into unknown area. More... | |
bool | collisionExists (const geometry_msgs::Pose2D &newPose, const geometry_msgs::Pose2D &collisionPoint) |
Checks the robot collision -2b changed-. More... | |
bool | collisionExistsNoPath (const geometry_msgs::Pose2D &newPose) |
Checks the robot collision -2b changed-. More... | |
std::vector< std::pair< int, int > > | getPointsBetween (int x1, int y1, int x2, int y2) |
Robot's previous movement direction in X Axis. More... | |
void | publishTransforms (const ros::TimerEvent &) |
Publishes the tf transforms every with 10Hz. More... | |
Private Attributes | |
geometry_msgs::Pose2D | _currentPose |
Holds robots previous pose. More... | |
std::vector< std::pair< float, float > > | _footprint |
nav_msgs::OccupancyGrid | _map |
ROS tf transform broadcaster. More... | |
ros::Subscriber | _mapSubscriber |
< ROS subscriber for map More... | |
MotionControllerPtr | _motionControllerPtr |
Actionlib client for registering the robot. More... | |
ros::ServiceServer | _moveRobotService |
Container for robot sensors. More... | |
ros::Publisher | _odomPublisher |
Holds robots current pose. More... | |
bool | _previousMovementXAxis |
Robot's previous movement direction in Y Axis. More... | |
bool | _previousMovementYAxis |
geometry_msgs::Pose2D | _previousPose |
Pointer of a motion controller. More... | |
RegisterRobotClientPtr | _registerClientPtr |
The robot footprint in points (row * 10000 + col) More... | |
SensorPtrVector | _sensors |
The occupancy grid map. More... | |
tf::TransformBroadcaster | _tfBroadcaster |
Odometry Publisher. More... | |
ros::Timer | _tfTimer |
ROS service server to move robot. More... | |
Additional Inherited Members | |
Protected Member Functions inherited from nodelet::Nodelet | |
ros::CallbackQueueInterface & | getMTCallbackQueue () const |
ros::NodeHandle & | getMTNodeHandle () const |
ros::NodeHandle & | getMTPrivateNodeHandle () const |
const V_string & | getMyArgv () const |
const std::string & | getName () const |
ros::NodeHandle & | getNodeHandle () const |
ros::NodeHandle & | getPrivateNodeHandle () const |
const M_string & | getRemappingArgs () const |
ros::CallbackQueueInterface & | getSTCallbackQueue () const |
std::string | getSuffixedName (const std::string &suffix) const |
Represents one robot in STDR. Inherts publicly from nodelet::Nodelet.
Definition at line 59 of file stdr_robot.h.
Robot::Robot | ( | void | ) |
Robot::~Robot | ( | void | ) |
|
private |
Checks the robot's reposition into unknown area.
newPose | [const geometry_msgs::Pose2D] The pose for the robot to be moved to |
newPose | [const geometry_msgs::Pose2D] The pose for the robot to be moved to |
Definition at line 249 of file stdr_robot.cpp.
|
private |
Checks the robot collision -2b changed-.
Definition at line 301 of file stdr_robot.cpp.
|
private |
Checks the robot collision -2b changed-.
Definition at line 215 of file stdr_robot.cpp.
|
private |
Robot's previous movement direction in X Axis.
Returns the points between two points.
x1 | : The x coord of the first point |
y1 | : The y coord of the first point |
x2 | : The x coord of the second point |
y2 | : The y coord of the second point |
Definition at line 276 of file stdr_robot.cpp.
void Robot::initializeRobot | ( | const actionlib::SimpleClientGoalState & | state, |
const stdr_msgs::RegisterRobotResultConstPtr | result | ||
) |
Initializes the robot after on registering it to server.
state | [const actionlib::SimpleClientGoalState&] State of action |
result | [const stdr_msgs::RegisterRobotResultConstPtr] Action result of registering the robot |
Definition at line 74 of file stdr_robot.cpp.
void Robot::mapCallback | ( | const nav_msgs::OccupancyGridConstPtr & | msg | ) |
Callback for getting the occupancy grid map.
msg | [const nav_msgs::OccupancyGridConstPtr&] The occupancy grid map |
Definition at line 183 of file stdr_robot.cpp.
bool Robot::moveRobotCallback | ( | stdr_msgs::MoveRobot::Request & | req, |
stdr_msgs::MoveRobot::Response & | res | ||
) |
The callback of the re-place robot service.
req | [stdr_msgs::MoveRobot::Request&] The service request |
res | [stdr_msgs::MoveRobot::Response&] The service result |
Definition at line 194 of file stdr_robot.cpp.
|
virtual |
Initializes the robot and gets the environment occupancy grid map.
Implements nodelet::Nodelet.
Definition at line 43 of file stdr_robot.cpp.
|
private |
Publishes the tf transforms every with 10Hz.
< Odometry
< Sensors tf
Definition at line 413 of file stdr_robot.cpp.
|
private |
Holds robots previous pose.
Definition at line 157 of file stdr_robot.h.
|
private |
Definition at line 169 of file stdr_robot.h.
|
private |
ROS tf transform broadcaster.
Definition at line 148 of file stdr_robot.h.
|
private |
< ROS subscriber for map
ROS timer to publish tf transforms (10Hz)
Definition at line 136 of file stdr_robot.h.
|
private |
Actionlib client for registering the robot.
Definition at line 163 of file stdr_robot.h.
|
private |
Container for robot sensors.
Definition at line 142 of file stdr_robot.h.
|
private |
Holds robots current pose.
Definition at line 154 of file stdr_robot.h.
|
private |
Robot's previous movement direction in Y Axis.
Definition at line 175 of file stdr_robot.h.
|
private |
Definition at line 178 of file stdr_robot.h.
|
private |
Pointer of a motion controller.
Definition at line 160 of file stdr_robot.h.
|
private |
The robot footprint in points (row * 10000 + col)
Definition at line 166 of file stdr_robot.h.
|
private |
The occupancy grid map.
Definition at line 145 of file stdr_robot.h.
|
private |
Odometry Publisher.
Definition at line 151 of file stdr_robot.h.
|
private |
ROS service server to move robot.
Definition at line 139 of file stdr_robot.h.