31 #include <nav_msgs/Odometry.h>
32 #include <nav_msgs/Path.h>
34 #include <move_base_msgs/MoveBaseAction.h>
35 #include <actionlib/client/simple_action_client.h>
36 #include <tf/transform_datatypes.h>
37 #include <tf/transform_broadcaster.h>
38 #include <tf/transform_listener.h>
40 #include <geometry_msgs/PoseWithCovarianceStamped.h>
42 #include <ar_pose/ARMarkers.h>
43 #include <ar_pose/ARMarker.h>
45 #include "std_msgs/String.h"
56 move_base_msgs::MoveBaseGoal
goal;
77 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
MoveBaseClient;
99 double dist2 = ((x1-x2)*(x1-x2))+((y1-y2)*(y1-y2));
100 return (sqrt(dist2));
104 #endif // HUMAN_PROC_H
geometry_msgs::PoseStamped next_pose
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient
ros::Subscriber pose_subscriber
geometry_msgs::PoseStamped robot_goal
void target_pose_callback(const nav_msgs::Odometry::ConstPtr &msg)
tf::StampedTransform transform
move_base_msgs::MoveBaseGoal goal
void robot_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
void ar_pose_callback(const ar_pose::ARMarkers::ConstPtr &markers)
void dyn_objects_callback(const trajectory_simulator::TrajectoryObservation::ConstPtr &dyn_objects)
geometry_msgs::PoseStamped source_pose
geometry_msgs::PoseWithCovarianceStamped robot_pose
ros::Subscriber dyn_objects_subscriber
geometry_msgs::Twist cmd_vel
nav_msgs::Path leader_path
ros::Subscriber robot_goal_subscriber
ros::Subscriber leader_goal_subscriber
double euclidean_dist(double x1, double y1, double x2, double y2)
double leader2goal_distance
ros::Subscriber ar_pose_sub
tf::TransformListener * listener
geometry_msgs::PoseStamped leader_pose
ros::Publisher robot_cmd_vel
boost::shared_ptr< ::trajectory_simulator::TrajectoryObservation_< ContainerAllocator > const > ConstPtr
double robot2goal_distance
geometry_msgs::PoseStamped leader_goal[4]
void leader_goal_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
ros::Subscriber robot_pose_subscriber
trajectory_simulator::TrajectoryObservation candidate_
ros::Publisher pathPublisher
geometry_msgs::PoseStamped target_pose
ros::Publisher next_posePublisher
void robot_goal_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)