Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _force_navigation_alg_node_h_
00026 #define _force_navigation_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "force_navigation_alg.h"
00030 #include "force_navigation.h"
00031 #include <tf/transform_listener.h>
00032 #include <geometry_msgs/PointStamped.h>
00033
00034
00035 #include <std_msgs/Float64MultiArray.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <iri_perception_msgs/peopleTrackingArray.h>
00038 #include <visualization_msgs/MarkerArray.h>
00039
00040
00041
00042
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <actionlib/client/terminal_state.h>
00045 #include <move_base_msgs/MoveBaseAction.h>
00046
00051 class ForceNavigationAlgNode : public algorithm_base::IriBaseAlgorithm<ForceNavigationAlgorithm>
00052 {
00053 private:
00054
00055 ros::Publisher navigation_work_publisher_;
00056 std_msgs::Float64MultiArray Float64_msg_;
00057 ros::Publisher robot_simulated_pose_publisher_;
00058 geometry_msgs::PoseStamped PoseStamped_msg_;
00059 ros::Publisher trajectories_publisher_;
00060 visualization_msgs::MarkerArray MarkerArray_trajectories_msg_;
00061 ros::Publisher forces_publisher_;
00062 visualization_msgs::MarkerArray MarkerArray_forces_msg_;
00063 ros::Publisher destinations_publisher_;
00064 visualization_msgs::MarkerArray MarkerArray_destinations_msg_;
00065 visualization_msgs::Marker traj_marker_, traj_robot_marker_, robot_marker_;
00066 visualization_msgs::Marker target_marker_;
00067 visualization_msgs::Marker dest_marker_;
00068 visualization_msgs::Marker text_marker_;
00069 visualization_msgs::Marker force_marker_, force_goal_marker_, force_goal2_marker_, force_int_person_marker_,
00070 force_int_robot_marker_, force_obstacle_map_marker_, force_obstacle_laser_marker_;
00071 ros::Publisher velocity_publisher_;
00072 geometry_msgs::Twist Twist_msg_;
00073
00074
00075 ros::Subscriber tracks_subscriber_;
00076 void tracks_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg);
00077 CMutex tracks_mutex_;
00078
00079
00080
00081
00082
00083
00084
00085
00086 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_robot_client_;
00087 move_base_msgs::MoveBaseGoal move_robot_goal_;
00088 void move_robotMakeActionRequest();
00089 void move_robotDone(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
00090 void move_robotActive();
00091 void move_robotFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
00092
00093
00094
00095 Cforce_navigation force_navigation_;
00096 void init_node();
00097 void vis_trajectories();
00098 void clear_force_markers();
00099 void vis_destinations();
00100 std::string target_frame_, robot_frame_;
00101 bool move_base_mode_, robot_sim_, robot_stop_;
00102 tf::TransformListener tf_listener_,tf_main_;
00103 double robot_x_ini_,robot_y_ini_;
00104 Spose robot_desired_position_;
00105 string force_map_path_,destination_map_path_;
00106
00107 public:
00114 ForceNavigationAlgNode(void);
00115
00122 ~ForceNavigationAlgNode(void);
00123
00124 protected:
00137 void mainNodeThread(void);
00138
00151 void node_config_update(Config &config, uint32_t level);
00152
00159 void addNodeDiagnostics(void);
00160
00161
00162
00163
00164 };
00165
00166 #endif