#include <ftc_planner.h>
Public Member Functions | |
bool | computeVelocityCommands (geometry_msgs::Twist &cmd_vel) |
Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. | |
FTCPlanner () | |
void | initialize (std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros) |
Constructs the local planner. | |
bool | isGoalReached () |
Check if the goal pose has been achieved by the local planner. | |
bool | setPlan (const std::vector< geometry_msgs::PoseStamped > &plan) |
Set the plan that the local planner is following. | |
~FTCPlanner () | |
Private Member Functions | |
double | calculateGlobalPlanAngle (tf::Stamped< tf::Pose > current_pose, const std::vector< geometry_msgs::PoseStamped > &plan, int points) |
Calculate the orientation of the global plan. | |
bool | checkCollision (int max_points) |
Check if the considerd points are in local collision. | |
int | checkMaxAngle (int points, tf::Stamped< tf::Pose > current_pose) |
Goes backward along global plan the max angle whith sim_time and max_rotation_vel allow. | |
int | checkMaxDistance (tf::Stamped< tf::Pose > current_pose) |
Goes along global plan the max distance whith sim_time and max_x_vel allow. | |
int | driveToward (tf::Stamped< tf::Pose > current_pose, geometry_msgs::Twist &cmd_vel) |
Drive along the global plan and calculate the velocity. | |
void | publishPlan (int max_point) |
Publish the global plan for visulatation. | |
void | reconfigureCB (FTCPlannerConfig &config, uint32_t level) |
Reconfigure config_. | |
bool | rotateToOrientation (double angle, geometry_msgs::Twist &cmd_vel, double accuracy) |
Rotation at place. | |
Private Attributes | |
double | cmd_vel_angular_z_ |
double | cmd_vel_angular_z_rotate_ |
double | cmd_vel_linear_x_ |
ftc_local_planner::FTCPlannerConfig | config_ |
costmap_2d::Costmap2DROS * | costmap_ros_ |
ftc_local_planner::FTCPlannerConfig | default_config_ |
dynamic_reconfigure::Server < FTCPlannerConfig > * | dsrv_ |
bool | first_setPlan_ |
std::vector < geometry_msgs::PoseStamped > | global_plan_ |
tf::Stamped< tf::Pose > | goal_pose_ |
bool | goal_reached_ |
JoinCostmap * | joinCostmap_ |
ros::Publisher | local_plan_publisher_ |
tf::Stamped< tf::Pose > | old_goal_pose_ |
bool | rotate_to_global_plan_ |
bool | stand_at_goal_ |
tf::TransformListener * | tf_ |
std::vector < geometry_msgs::PoseStamped > | transformed_global_plan_ |
Definition at line 42 of file ftc_planner.h.
Definition at line 29 of file ftc_planner.cpp.
Definition at line 516 of file ftc_planner.cpp.
double ftc_local_planner::FTCPlanner::calculateGlobalPlanAngle | ( | tf::Stamped< tf::Pose > | current_pose, |
const std::vector< geometry_msgs::PoseStamped > & | plan, | ||
int | points | ||
) | [private] |
Calculate the orientation of the global plan.
current | robot pose |
global | plan |
number | of points which used for calculation |
Definition at line 228 of file ftc_planner.cpp.
bool ftc_local_planner::FTCPlanner::checkCollision | ( | int | max_points | ) | [private] |
Check if the considerd points are in local collision.
points | of global plan which are considerd. |
Definition at line 457 of file ftc_planner.cpp.
int ftc_local_planner::FTCPlanner::checkMaxAngle | ( | int | points, |
tf::Stamped< tf::Pose > | current_pose | ||
) | [private] |
Goes backward along global plan the max angle whith sim_time and max_rotation_vel allow.
point | where starts to go backward |
current | pose of the robot |
Definition at line 210 of file ftc_planner.cpp.
int ftc_local_planner::FTCPlanner::checkMaxDistance | ( | tf::Stamped< tf::Pose > | current_pose | ) | [private] |
Goes along global plan the max distance whith sim_time and max_x_vel allow.
current | pose of the robot |
Definition at line 179 of file ftc_planner.cpp.
bool ftc_local_planner::FTCPlanner::computeVelocityCommands | ( | geometry_msgs::Twist & | cmd_vel | ) | [virtual] |
Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
cmd_vel | Will be filled with the velocity command to be passed to the robot base |
Implements nav_core::BaseLocalPlanner.
Definition at line 107 of file ftc_planner.cpp.
int ftc_local_planner::FTCPlanner::driveToward | ( | tf::Stamped< tf::Pose > | current_pose, |
geometry_msgs::Twist & | cmd_vel | ||
) | [private] |
Drive along the global plan and calculate the velocity.
current | pose of the robot |
velocity | message |
Definition at line 339 of file ftc_planner.cpp.
void ftc_local_planner::FTCPlanner::initialize | ( | std::string | name, |
tf::TransformListener * | tf, | ||
costmap_2d::Costmap2DROS * | costmap_ros | ||
) | [virtual] |
Constructs the local planner.
name | The name to give this instance of the local planner |
tf | A pointer to a transform listener |
costmap_ros | The cost map to use for assigning costs to local plans |
Implements nav_core::BaseLocalPlanner.
Definition at line 33 of file ftc_planner.cpp.
bool ftc_local_planner::FTCPlanner::isGoalReached | ( | ) | [virtual] |
Check if the goal pose has been achieved by the local planner.
Implements nav_core::BaseLocalPlanner.
Definition at line 448 of file ftc_planner.cpp.
void ftc_local_planner::FTCPlanner::publishPlan | ( | int | max_point | ) | [private] |
Publish the global plan for visulatation.
points | where jused to calculate plan. |
Definition at line 492 of file ftc_planner.cpp.
void ftc_local_planner::FTCPlanner::reconfigureCB | ( | FTCPlannerConfig & | config, |
uint32_t | level | ||
) | [private] |
Reconfigure config_.
Definition at line 57 of file ftc_planner.cpp.
bool ftc_local_planner::FTCPlanner::rotateToOrientation | ( | double | angle, |
geometry_msgs::Twist & | cmd_vel, | ||
double | accuracy | ||
) | [private] |
Rotation at place.
angle | which is to rotate |
velocity | message which is calculate for rotation |
accuracy | of orientation |
Definition at line 253 of file ftc_planner.cpp.
bool ftc_local_planner::FTCPlanner::setPlan | ( | const std::vector< geometry_msgs::PoseStamped > & | plan | ) | [virtual] |
Set the plan that the local planner is following.
plan | The plan to pass to the local planner |
Implements nav_core::BaseLocalPlanner.
Definition at line 67 of file ftc_planner.cpp.
double ftc_local_planner::FTCPlanner::cmd_vel_angular_z_ [private] |
Definition at line 171 of file ftc_planner.h.
double ftc_local_planner::FTCPlanner::cmd_vel_angular_z_rotate_ [private] |
Definition at line 167 of file ftc_planner.h.
double ftc_local_planner::FTCPlanner::cmd_vel_linear_x_ [private] |
Definition at line 169 of file ftc_planner.h.
ftc_local_planner::FTCPlannerConfig ftc_local_planner::FTCPlanner::config_ [private] |
Definition at line 158 of file ftc_planner.h.
Definition at line 140 of file ftc_planner.h.
ftc_local_planner::FTCPlannerConfig ftc_local_planner::FTCPlanner::default_config_ [private] |
Definition at line 156 of file ftc_planner.h.
dynamic_reconfigure::Server<FTCPlannerConfig>* ftc_local_planner::FTCPlanner::dsrv_ [private] |
Definition at line 154 of file ftc_planner.h.
bool ftc_local_planner::FTCPlanner::first_setPlan_ [private] |
Definition at line 146 of file ftc_planner.h.
std::vector<geometry_msgs::PoseStamped> ftc_local_planner::FTCPlanner::global_plan_ [private] |
Definition at line 142 of file ftc_planner.h.
Definition at line 148 of file ftc_planner.h.
bool ftc_local_planner::FTCPlanner::goal_reached_ [private] |
Definition at line 160 of file ftc_planner.h.
Definition at line 173 of file ftc_planner.h.
Definition at line 164 of file ftc_planner.h.
Definition at line 150 of file ftc_planner.h.
bool ftc_local_planner::FTCPlanner::rotate_to_global_plan_ [private] |
Definition at line 152 of file ftc_planner.h.
bool ftc_local_planner::FTCPlanner::stand_at_goal_ [private] |
Definition at line 162 of file ftc_planner.h.
Definition at line 138 of file ftc_planner.h.
std::vector<geometry_msgs::PoseStamped> ftc_local_planner::FTCPlanner::transformed_global_plan_ [private] |
Definition at line 144 of file ftc_planner.h.