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_learning_alg_node_h_
00026 #define _force_navigation_learning_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "force_navigation_learning_alg.h"
00030 #include "force_navigation_learning.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
00048 class ForceNavigationLearningAlgNode : public algorithm_base::IriBaseAlgorithm<ForceNavigationLearningAlgorithm>
00049 {
00050 private:
00051
00052 ros::Publisher learning_data_publisher_;
00053 std_msgs::Float64MultiArray Float64_msg_;
00054 ros::Publisher robot_simulated_pose_publisher_;
00055 geometry_msgs::PoseStamped PoseStamped_msg_;
00056 ros::Publisher trajectories_publisher_;
00057 visualization_msgs::MarkerArray MarkerArray_trajectories_msg_;
00058 ros::Publisher forces_publisher_;
00059 visualization_msgs::MarkerArray MarkerArray_forces_msg_;
00060 ros::Publisher destinations_publisher_;
00061 visualization_msgs::MarkerArray MarkerArray_destinations_msg_;
00062 visualization_msgs::Marker traj_marker_, traj_robot_marker_, robot_marker_;
00063 visualization_msgs::Marker target_marker_;
00064 visualization_msgs::Marker dest_marker_;
00065 visualization_msgs::Marker text_marker_;
00066 visualization_msgs::Marker force_marker_, force_goal_marker_, force_goal2_marker_, force_int_person_marker_,
00067 force_int_robot_marker_, force_obstacle_map_marker_, force_obstacle_laser_marker_;
00068
00069
00070 ros::Subscriber tracks_subscriber_;
00071 void tracks_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg);
00072 CMutex tracks_mutex_;
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083 Cforce_navigation_learning force_navigation_;
00084 void init_node();
00085 void vis_trajectories();
00086 void clear_force_markers();
00087 void vis_destinations();
00088 std::string target_frame_, robot_frame_;
00089 tf::TransformListener tf_listener_;
00090 double robot_x_ini_,robot_y_ini_;
00091 Spose robot_desired_position_;
00092 string force_map_path_, destination_map_path_;
00093
00094 public:
00101 ForceNavigationLearningAlgNode(void);
00102
00109 ~ForceNavigationLearningAlgNode(void);
00110
00111 protected:
00124 void mainNodeThread(void);
00125
00138 void node_config_update(Config &config, uint32_t level);
00139
00146 void addNodeDiagnostics(void);
00147
00148
00149
00150
00151 };
00152
00153 #endif