#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.
|
private |
Enumerator | |
---|---|
NONE | |
GOAL | |
LOOP |
Definition at line 51 of file waypoints_navi.hpp.
|
private |
Enumerator | |
---|---|
IDLE | |
START | |
ACTIVE | |
COMPLETED |
Definition at line 56 of file waypoints_navi.hpp.
yocs::WaypointsGoalNode::WaypointsGoalNode | ( | ) |
Definition at line 17 of file waypoints_navi.cpp.
yocs::WaypointsGoalNode::~WaypointsGoalNode | ( | ) |
Definition at line 26 of file waypoints_navi.cpp.
|
private |
Definition at line 151 of file waypoints_navi.cpp.
|
private |
Definition at line 364 of file waypoints_navi.cpp.
|
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.
|
private |
Definition at line 377 of file waypoints_navi.cpp.
|
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.
|
private |
Definition at line 63 of file waypoints_navi.hpp.
|
private |
Definition at line 62 of file waypoints_navi.hpp.
|
private |
Definition at line 71 of file waypoints_navi.hpp.
|
private |
Definition at line 64 of file waypoints_navi.hpp.
|
private |
Definition at line 82 of file waypoints_navi.hpp.
enum { ... } yocs::WaypointsGoalNode::mode_ |
|
private |
Definition at line 84 of file waypoints_navi.hpp.
|
private |
Definition at line 80 of file waypoints_navi.hpp.
|
private |
Definition at line 49 of file waypoints_navi.hpp.
|
private |
Definition at line 65 of file waypoints_navi.hpp.
enum { ... } yocs::WaypointsGoalNode::state_ |
|
private |
Definition at line 81 of file waypoints_navi.hpp.
|
private |
Definition at line 76 of file waypoints_navi.hpp.
|
private |
Definition at line 74 of file waypoints_navi.hpp.
|
private |
Definition at line 78 of file waypoints_navi.hpp.
|
private |
Definition at line 68 of file waypoints_navi.hpp.
|
private |
Definition at line 69 of file waypoints_navi.hpp.
|
private |
Definition at line 77 of file waypoints_navi.hpp.
|
private |
Definition at line 66 of file waypoints_navi.hpp.
|
private |
Definition at line 73 of file waypoints_navi.hpp.