planner_ros

An inheritance of the Planner class to be used in the ROS environment.

Subscribes to: /elevation_mapping/elevation_map_raw

Publishes to: /slope_constrained_planner/path

Uses Action: /slope_constrained_planner/plan_to_goal

namespace slope_constrained_planner

class PlannerRos : protected slope_constrained_planner::Planner

Public Functions

~PlannerRos()

Destructor.

explicit PlannerRos(const ros::NodeHandle &nh)

Constructor.

Parameters:

nh – NodeHandler object

Public Members

std::unique_ptr<grid_map::GridMap> map

grid_map_msgs::GridMapInfo info

Protected Types

using PlanningActionServer = actionlib::SimpleActionServer<slope_constrained_planner_msgs::PlanToGoalAction>

using Feedback = slope_constrained_planner_msgs::PlanToGoalFeedback

using FeedbackStatus = Feedback::_status_type

Protected Functions

void stopPlanningContinuously()

Stops the planning thread by putting false value in the corresponding class attribute.

virtual void planContinuouslyThread()

The main thread in this class which tries to plan whithin the defined time from robot’s current position to the target point until a valid path is found.

virtual void publishPathThread()

Publishes the path to corresponding topic continusly, once a valid path is found by the planner thread.

void publishPhiThread()

Publishes phi to corresponding topic continusly.

virtual void planFromCurrentRobotPose()

Tries to plan from current robot’s pose to the target using UpdateMapAndPlanFromCurrentRobotPose() function.

bool getCurrentRobotPose(geometry_msgs::PoseStamped *pose) const

Gets robot current position from the tf tree.

Parameters:

pose – Robot position

void publishFeedback(FeedbackStatus feedback) const

Publishes the planner server feedback.

Parameters:

feedback – Feedback to be published

void mapCallback(const grid_map_msgs::GridMapConstPtr &map_msg)

Callback function for subscribing the grid map topic.

Parameters:

map_msg – Grid map data

void cancelGoalCallback()

Callback function for canceling the goal in the planner server.

void goalCallback()

Callback function for when a new pose is available in the planner server.

void goalPoseCallback(const slope_constrained_planner_msgs::PlanToGoalGoalConstPtr &goal)

Helper function to be used in goalCallback() function. Gets the goal and trigers the planner thread to find the path.

Parameters:

goal – Goal message which is defiened in the custom action file

virtual void publishPath(nav_msgs::Path path)

Publishes the found path to corresponding topic.

Parameters:

path – The path which is found by the planner

virtual void publishPhi(std_msgs::Float64 phi)

Publishes the value of phi in degrees.

Parameters:

phi – The value of phi

virtual void publishTheta(std_msgs::Float64 theta)

Publishes the value of theta in degrees.

Parameters:

theta – The value of theta

void updateMap()

void publishMap() const

Publiesh the map data to corresponding topic.

PlannerStatus updateMapAndPlanFromCurrentRobotPose(const ob::ScopedState<> &goal)

Uses updateMap() function and then plan(start, goal) function from the base class. Once the path is found, calls publishMap() function. This function is used in the planContinueslyThread() function.

Parameters:

goal – The goal state to be set in the planner

Protected Attributes

ros::NodeHandle nh_

Node handler attribute.

ros::Subscriber map_sub_

Map subscriber attribute.

std::unique_ptr<PlanningActionServer> plan_act_srv_

Planner action server attribute.

double planning_time_start_

Planning time duration.

double planning_time_end_

ros::Publisher path_pub_

Path publisher attribute.

ros::Publisher phi_pub_

Phi publisher attribute.

ros::Publisher theta_pub_

Theta publisher attribute.

ros::Publisher map_pub_

Map publisher attribute.

tf2_ros::Buffer tf_buffer_

tf buffer attribute

tf2_ros::TransformListener tf_listener_ = {tf_buffer_}

tf listener attribute

geometry_msgs::PoseStamped pose_goal_

Goal position attribute.

mutable std::mutex pose_goal_mutex_

Goal position process mutex attribute.

std::atomic<bool> planning_continuously_ = {false}

Set the planner thread to whether perform or not.

std::atomic<bool> thread_built_ = {false}

Set the planning thread built status.

std::atomic<bool> publish_path_ = {false}

Sets the publish path thread to whether perform or not.

std::thread continuous_planning_thread_

Planner thread attribute.

std::thread publish_path_thread_

Publish path thread attribute.

std::thread publish_phi_thread_

Publish phi thread attribute.

std::mutex planning_thread_mutex_

Planner thread process mutex attribute.

nav_msgs::Path plan_ros_

Found solution by the planner thread attribute.

grid_map_msgs::GridMapInfo planning_map_info_

Grid map info attribute.

struct slope_constrained_planner::PlannerRos::[anonymous] map_queue_

Grid map data queue attribute.

mutable std::mutex map_queue_mutex_

Grid map process mutex attribute.

Converter converter_

An object from converter class attribute.