Public Member Functions | Private Member Functions | Private Attributes
ftc_local_planner::FTCPlanner Class Reference

#include <ftc_planner.h>

Inheritance diagram for ftc_local_planner::FTCPlanner:
Inheritance graph
[legend]

List of all members.

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::Costmap2DROScostmap_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::Posegoal_pose_
bool goal_reached_
JoinCostmapjoinCostmap_
ros::Publisher local_plan_publisher_
tf::Stamped< tf::Poseold_goal_pose_
bool rotate_to_global_plan_
bool stand_at_goal_
tf::TransformListenertf_
std::vector
< geometry_msgs::PoseStamped > 
transformed_global_plan_

Detailed Description

Definition at line 42 of file ftc_planner.h.


Constructor & Destructor Documentation

Definition at line 29 of file ftc_planner.cpp.

Definition at line 516 of file ftc_planner.cpp.


Member Function Documentation

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.

Parameters:
currentrobot pose
globalplan
numberof 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.

Parameters:
pointsof global plan which are considerd.
Returns:
true if no collision.

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.

Parameters:
pointwhere starts to go backward
currentpose of the robot
Returns:
max point of the global plan with can reached

Definition at line 210 of file ftc_planner.cpp.

Goes along global plan the max distance whith sim_time and max_x_vel allow.

Parameters:
currentpose of the robot
Returns:
max point of the global plan with can reached

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.

Parameters:
cmd_velWill be filled with the velocity command to be passed to the robot base
Returns:
True if a valid velocity command was found, false otherwise

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.

Parameters:
currentpose of the robot
velocitymessage
Returns:
number of points of global plan which are used

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.

Parameters:
nameThe name to give this instance of the local planner
tfA pointer to a transform listener
costmap_rosThe cost map to use for assigning costs to local plans

Implements nav_core::BaseLocalPlanner.

Definition at line 33 of file ftc_planner.cpp.

Check if the goal pose has been achieved by the local planner.

Returns:
True if achieved, false otherwise

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.

Parameters:
pointswhere 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.

Parameters:
anglewhich is to rotate
velocitymessage which is calculate for rotation
accuracyof orientation
Returns:
true if rotate, false if rotation goal reached

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.

Parameters:
planThe plan to pass to the local planner
Returns:
True if the plan was updated successfully, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 67 of file ftc_planner.cpp.


Member Data Documentation

Definition at line 171 of file ftc_planner.h.

Definition at line 167 of file ftc_planner.h.

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.

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.

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.

Definition at line 152 of file ftc_planner.h.

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.


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


asr_ftc_local_planner
Author(s): Marek Felix
autogenerated on Wed Jun 19 2019 19:38:18