#include <waypoints_navi.hpp>
Public Member Functions | |
bool | init () |
void | navCtrlCB (const yocs_msgs::NavigationControl::ConstPtr &nav_ctrl) |
void | spin () |
void | trajectoriesCB (const yocs_msgs::TrajectoryList::ConstPtr &trajs) |
void | waypointsCB (const yocs_msgs::WaypointList::ConstPtr &wps) |
WaypointsGoalNode () | |
~WaypointsGoalNode () | |
Private Types | |
enum | { NONE = 0, GOAL, LOOP } |
enum | { IDLE = 0, START, ACTIVE, COMPLETED } |
Private Member Functions | |
bool | cancelAllGoals (double timeout=2.0) |
bool | equals (const geometry_msgs::PoseStamped &a, const geometry_msgs::PoseStamped &b) |
bool | equals (const geometry_msgs::Point &a, const geometry_msgs::Point &b) |
void | publishStatusUpdate (const uint8_t &status) |
void | resetWaypoints () |
Private Attributes | |
double | close_enough_ |
double | frequency_ |
geometry_msgs::PoseStamped | goal_ |
double | goal_timeout_ |
bool | idle_status_update_sent_ |
enum yocs::WaypointsGoalNode:: { ... } | mode_ |
actionlib::SimpleActionClient < move_base_msgs::MoveBaseAction > | move_base_ac_ |
ros::Subscriber | nav_ctrl_sub_ |
const geometry_msgs::PoseStamped | NOWHERE |
std::string | robot_frame_ |
enum yocs::WaypointsGoalNode:: { ... } | state_ |
ros::Publisher | status_pub_ |
tf::TransformListener | tf_listener_ |
yocs_msgs::TrajectoryList | traj_list_ |
ros::Subscriber | trajectories_sub_ |
std::vector < geometry_msgs::PoseStamped > | waypoints_ |
std::vector < geometry_msgs::PoseStamped > ::iterator | waypoints_it_ |
ros::Subscriber | waypoints_sub_ |
std::string | world_frame_ |
yocs_msgs::WaypointList | wp_list_ |
Definition at line 32 of file waypoints_navi.hpp.
anonymous enum [private] |
Definition at line 51 of file waypoints_navi.hpp.
anonymous enum [private] |
Definition at line 56 of file waypoints_navi.hpp.
Definition at line 17 of file waypoints_navi.cpp.
Definition at line 26 of file waypoints_navi.cpp.
bool yocs::WaypointsGoalNode::cancelAllGoals | ( | double | timeout = 2.0 | ) | [private] |
Definition at line 151 of file waypoints_navi.cpp.
bool yocs::WaypointsGoalNode::equals | ( | const geometry_msgs::PoseStamped & | a, |
const geometry_msgs::PoseStamped & | b | ||
) | [private] |
Definition at line 364 of file waypoints_navi.cpp.
bool yocs::WaypointsGoalNode::equals | ( | const geometry_msgs::Point & | a, |
const geometry_msgs::Point & | b | ||
) | [private] |
Definition at line 372 of file waypoints_navi.cpp.
bool yocs::WaypointsGoalNode::init | ( | ) |
Definition at line 29 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::navCtrlCB | ( | const yocs_msgs::NavigationControl::ConstPtr & | nav_ctrl | ) |
Definition at line 68 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::publishStatusUpdate | ( | const uint8_t & | status | ) | [private] |
Definition at line 377 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::resetWaypoints | ( | ) | [private] |
Definition at line 180 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::spin | ( | ) |
Definition at line 189 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::trajectoriesCB | ( | const yocs_msgs::TrajectoryList::ConstPtr & | trajs | ) |
Definition at line 62 of file waypoints_navi.cpp.
void yocs::WaypointsGoalNode::waypointsCB | ( | const yocs_msgs::WaypointList::ConstPtr & | wps | ) |
Definition at line 56 of file waypoints_navi.cpp.
double yocs::WaypointsGoalNode::close_enough_ [private] |
Definition at line 63 of file waypoints_navi.hpp.
double yocs::WaypointsGoalNode::frequency_ [private] |
Definition at line 62 of file waypoints_navi.hpp.
geometry_msgs::PoseStamped yocs::WaypointsGoalNode::goal_ [private] |
Definition at line 71 of file waypoints_navi.hpp.
double yocs::WaypointsGoalNode::goal_timeout_ [private] |
Definition at line 64 of file waypoints_navi.hpp.
bool yocs::WaypointsGoalNode::idle_status_update_sent_ [private] |
Definition at line 82 of file waypoints_navi.hpp.
enum { ... } yocs::WaypointsGoalNode::mode_ [private] |
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> yocs::WaypointsGoalNode::move_base_ac_ [private] |
Definition at line 84 of file waypoints_navi.hpp.
Definition at line 80 of file waypoints_navi.hpp.
const geometry_msgs::PoseStamped yocs::WaypointsGoalNode::NOWHERE [private] |
Definition at line 49 of file waypoints_navi.hpp.
std::string yocs::WaypointsGoalNode::robot_frame_ [private] |
Definition at line 65 of file waypoints_navi.hpp.
enum { ... } yocs::WaypointsGoalNode::state_ [private] |
Definition at line 81 of file waypoints_navi.hpp.
Definition at line 76 of file waypoints_navi.hpp.
yocs_msgs::TrajectoryList yocs::WaypointsGoalNode::traj_list_ [private] |
Definition at line 74 of file waypoints_navi.hpp.
Definition at line 78 of file waypoints_navi.hpp.
std::vector<geometry_msgs::PoseStamped> yocs::WaypointsGoalNode::waypoints_ [private] |
Definition at line 68 of file waypoints_navi.hpp.
std::vector<geometry_msgs::PoseStamped>::iterator yocs::WaypointsGoalNode::waypoints_it_ [private] |
Definition at line 69 of file waypoints_navi.hpp.
Definition at line 77 of file waypoints_navi.hpp.
std::string yocs::WaypointsGoalNode::world_frame_ [private] |
Definition at line 66 of file waypoints_navi.hpp.
yocs_msgs::WaypointList yocs::WaypointsGoalNode::wp_list_ [private] |
Definition at line 73 of file waypoints_navi.hpp.