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_robot_companion_learning_alg_node_h_
00026 #define _force_robot_companion_learning_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "force_robot_companion_learning_alg.h"
00030
00031
00032
00033 #include <std_msgs/Float64.h>
00034 #include <sensor_msgs/LaserScan.h>
00035 #include <geometry_msgs/PoseArray.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <geometry_msgs/PointStamped.h>
00038
00039 #include <visualization_msgs/MarkerArray.h>
00040 #include <iri_perception_msgs/peopleTrackingArray.h>
00041
00042
00043 #include <std_srvs/Empty.h>
00044
00045
00046
00047
00048
00049
00050 #include <tf/transform_listener.h>
00051 #include "nav/force_reactive_robot_companion_learning.h"
00052 #include <string>
00053
00054 using namespace std;
00055
00056
00061 class ForceRobotCompanionLearningAlgNode : public algorithm_base::IriBaseAlgorithm<ForceRobotCompanionLearningAlgorithm>
00062 {
00063 enum sim_state {Init_Sim=0, Find_Nearest, Test_Sim, End_Sim };
00064 enum learning_mode {Density=0, Initial_paramters };
00065 private:
00066
00067 ros::Publisher angle_robot_person_publisher_;
00068 std_msgs::Float64 angle_robot_person_msg_;
00069 ros::Publisher person_robot_dist_publisher_;
00070 std_msgs::Float64 person_robot_dist_msg_;
00071 ros::Publisher target_person_position_publisher_;
00072 geometry_msgs::PointStamped target_person_position_msg_;
00073 ros::Publisher robot_position_publisher_;
00074 geometry_msgs::PoseStamped robot_position_msg_;
00075 ros::Publisher force_param_publisher_;
00076 geometry_msgs::PointStamped force_param_msg_;
00077 ros::Publisher trajectories_publisher_;
00078 visualization_msgs::MarkerArray MarkerArray_trajectories_msg_;
00079 ros::Publisher forces_publisher_;
00080 visualization_msgs::MarkerArray MarkerArray_forces_msg_;
00081 ros::Publisher destinations_publisher_;
00082 visualization_msgs::MarkerArray MarkerArray_destinations_msg_;
00083 ros::Publisher predictions_publisher_;
00084 visualization_msgs::MarkerArray MarkerArray_predictions_msg_;
00085
00086 visualization_msgs::Marker traj_marker_, traj_robot_marker_, robot_marker_;
00087 visualization_msgs::Marker target_marker_;
00088 visualization_msgs::Marker dest_marker_;
00089 visualization_msgs::Marker pred_marker_;
00090 visualization_msgs::Marker text_marker_;
00091 visualization_msgs::Marker force_marker_, force_goal_marker_, force_goal2_marker_, force_int_person_marker_,
00092 force_int_robot_marker_, force_obstacle_map_marker_, force_obstacle_laser_marker_;
00093 void init_node();
00094 void vis_trajectories();
00095 void clear_force_markers();
00096 void vis_intentionality_prediction();
00097 void vis_destinations();
00098 void vis_predictions();
00099 void growing_density_test();
00100 void initial_params_learning();
00101
00102
00103 ros::Subscriber laser_subscriber_;
00104 CMutex laser_mutex_;
00105 ros::Subscriber dest_subscriber_;
00106 void dest_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
00107 CMutex dest_mutex_;
00108 ros::Subscriber tracks_subscriber_;
00109 void tracks_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg);
00110 CMutex tracks_mutex_;
00111
00112
00113
00114
00115
00116 ros::ServiceClient reset_client_;
00117 std_srvs::Empty reset_srv_;
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 Cforce_reactive_robot_companion_learning pred_;
00130 std::string target_frame_, robot_frame_;
00131 tf::TransformListener tf_listener_;
00132
00133
00134 SpointV_cov robot_desired_position_, target_followed_pose_;
00135 Sforce f1_, f2_, f3_, f4_, f5_, f_;
00136 ros::Time now_;
00137
00138 bool using_prediction_;
00139 double param_f1_, param_f2_, param_f3_;
00140 std::vector<int> prev_buttons_;
00141
00142
00143 int nearest_target_;
00144 string force_map_path_,destination_map_path_;
00145 double laser_radi_;
00146
00147
00148 sim_state simulation_state_;
00149 int number_of_virtual_people_,num_experiment_,timer_;
00150 FILE * file_results_out_;
00151 bool results_file_is_open_;
00152 string results_folder_;
00153 double robot_x_ini_,robot_y_ini_;
00154
00155
00156 learning_mode learning_mode_;
00157
00158 public:
00165 ForceRobotCompanionLearningAlgNode(void);
00166
00173 ~ForceRobotCompanionLearningAlgNode(void);
00174
00175 protected:
00188 void mainNodeThread(void);
00189
00202 void node_config_update(Config &config, uint32_t level);
00203
00210 void addNodeDiagnostics(void);
00211
00212
00213
00214
00215 };
00216
00217 #endif