27 #ifndef LOCAL_BEHAVIOR_CONTROLLER_NODE_H 28 #define LOCAL_BEHAVIOR_CONTROLLER_NODE_H 32 #include <nav_msgs/Path.h> 33 #include <geometry_msgs/PoseWithCovarianceStamped.h> 34 #include <tuw_multi_robot_msgs/Route.h> 35 #include <tuw_multi_robot_msgs/RobotInfo.h> 51 std::unique_ptr<ros::Rate>
rate_;
78 void subPoseCb(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& _pose);
79 void subRobotInfoCb(
const tuw_multi_robot_msgs::RobotInfo::ConstPtr& _robot_info);
80 void subRouteCb(
const tuw_multi_robot_msgs::Route::ConstPtr& _route);
92 #endif // LOCAL_BEHAVIOR_CONTROLLER_NODE_H
std::unique_ptr< ros::Rate > rate_
tuw_multi_robot_msgs::Route robot_route_
LocalBehaviorControllerNode(ros::NodeHandle &n)
std::string topic_robot_info_
double robotDefaultRadius_
ros::Subscriber subRoute_
ros::Publisher pubRobotInfo_
RobotStateObserver observer_
ros::NodeHandle n_param_
Node handler to the current node.
void subRouteCb(const tuw_multi_robot_msgs::Route::ConstPtr &_route)
ros::NodeHandle n_
Node handler to the root node.
void publishPath(std::vector< Eigen::Vector3d > _p)
ros::Subscriber subRobotInfo_
void subPoseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &_pose)
geometry_msgs::PoseWithCovariance robot_pose_
std::map< std::string, int > robot_steps_
void subRobotInfoCb(const tuw_multi_robot_msgs::RobotInfo::ConstPtr &_robot_info)