Go to the documentation of this file.00001 #include "people_simulation_alg_node.h"
00002
00003 PeopleSimulationAlgNode::PeopleSimulationAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<PeopleSimulationAlgorithm>(),
00005 n_persons_(0), simulation_mode_( PeopleSimulationAlgNode::Normal )
00006 {
00007
00008 this->loop_rate_ = 5;
00009 scene_.set_dt( 1.0 / 5.0 );
00010
00011 this->public_node_handle_.getParam("number_persons", n_persons_);
00012 this->public_node_handle_.getParam("force_map_path", force_map_path_);
00013 this->public_node_handle_.getParam("destination_map_path", destination_map_path_);
00014 this->public_node_handle_.getParam("remove_targets", remove_targets_);
00015
00016
00017
00018 this->tracksMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("tracksMarkers", 100);
00019 this->tracks_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::peopleTrackingArray>("tracks", 100);
00020
00021
00022 this->robot_pose_subscriber_ = this->public_node_handle_.subscribe("robot_pose", 100, &PeopleSimulationAlgNode::robot_pose_callback, this);
00023
00024
00025 this->reset_server_ = this->public_node_handle_.advertiseService("reset", &PeopleSimulationAlgNode::resetCallback, this);
00026
00027
00028
00029
00030
00031
00032
00033 init_sim();
00034 }
00035
00036 PeopleSimulationAlgNode::~PeopleSimulationAlgNode(void)
00037 {
00038
00039 }
00040
00041 void PeopleSimulationAlgNode::init_sim()
00042 {
00043
00044 scene_.set_number_virtual_people( n_persons_ );
00045 scene_.set_remove_targets( remove_targets_ );
00046 peopleTrackingArray_msg_.header.frame_id = "/map";
00047
00048
00049 if ( !scene_.read_destination_map( destination_map_path_.c_str() ) )
00050 {
00051 ROS_ERROR("Could not read map destinations file !!!");
00052 }
00053 else{
00054 ROS_WARN("read destinations map file : SUCCESS!!!");
00055 }
00056
00057
00058 if ( !scene_.read_force_map( force_map_path_.c_str() ) )
00059 {
00060 ROS_ERROR("Could not read map force file !!!");
00061 }
00062 else{
00063 ROS_WARN("read map force file : SUCCESS!!!");
00064 }
00065
00066
00067
00068
00069 track_marker_.header.frame_id = "/map";
00070 track_marker_.ns = "tracks";
00071 track_marker_.type = visualization_msgs::Marker::CYLINDER;
00072 track_marker_.action = visualization_msgs::Marker::ADD;
00073 track_marker_.lifetime = ros::Duration(1.0f);
00074 track_marker_.scale.x = 0.5;
00075 track_marker_.scale.y = 0.5;
00076 track_marker_.scale.z = 0.6;
00077 track_marker_.color.a = 0.6;
00078 track_marker_.color.r = 0.0;
00079 track_marker_.color.g = 1.0;
00080 track_marker_.color.b = 0.0;
00081 track_marker_.pose.position.z = 0.3;
00082
00083 id_marker_.header.frame_id = "/map";
00084 id_marker_.ns = "ids";
00085 id_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00086 id_marker_.action = visualization_msgs::Marker::ADD;
00087 id_marker_.lifetime = ros::Duration(1.0f);
00088 id_marker_.scale.z = 0.5;
00089 id_marker_.pose.position.z = 1.0;
00090 id_marker_.color.a = 0.5;
00091 id_marker_.color.r = 0.0;
00092 id_marker_.color.g = 0.0;
00093 id_marker_.color.b = 0.0;
00094
00095 }
00096
00097 void PeopleSimulationAlgNode::mainNodeThread(void)
00098 {
00099
00100
00101 peopleTrackingArray_msg_.header.stamp = ros::Time::now();
00102 peopleTrackingArray_msg_.peopleSet.clear();
00103
00104 iri_perception_msgs::peopleTracking person;
00105 person.covariances[0] = 1.0;
00106
00107
00108 this->alg_.lock();
00109 std::vector<SdetectionObservation> obs_scene;
00110 std::vector<SpointV_cov> scene_targets_pointV;
00111 std::vector<unsigned int> scene_targets_ids;
00112 obs_scene.push_back( SdetectionObservation(0, ros::Time::now().toSec() ));
00113 scene_.update_scene( obs_scene );
00114 scene_.get_scene_observation(scene_targets_pointV,scene_targets_ids);
00115 this->alg_.unlock();
00116 for( unsigned int i=0; i < scene_targets_ids.size(); i++ )
00117 {
00118
00119 person.targetId = scene_targets_ids[i];
00120 person.x = scene_targets_pointV[i].x;
00121 person.y = scene_targets_pointV[i].y;
00122 person.vy = scene_targets_pointV[i].vx;
00123 person.vx = scene_targets_pointV[i].vy;
00124 peopleTrackingArray_msg_.peopleSet.push_back(person);
00125 }
00126
00127 if( vis_mode_ )
00128 {
00129 MarkerArray_msg_.markers.clear();
00130 track_marker_.header.stamp = peopleTrackingArray_msg_.header.stamp;
00131 id_marker_.header.stamp = peopleTrackingArray_msg_.header.stamp;
00132 for( unsigned int i=0; i < scene_targets_ids.size(); i++ )
00133 {
00134 track_marker_.id = scene_targets_ids[i];
00135 track_marker_.pose.position.x = scene_targets_pointV[i].x;
00136 track_marker_.pose.position.y = scene_targets_pointV[i].y;
00137 MarkerArray_msg_.markers.push_back(track_marker_);
00138 id_marker_.id = scene_targets_ids[i];
00139 id_marker_.pose.position.x = scene_targets_pointV[i].x;
00140 id_marker_.pose.position.y = scene_targets_pointV[i].y;
00141 std::ostringstream target_id;
00142 target_id << id_marker_.id;
00143 id_marker_.text = target_id.str();
00144 MarkerArray_msg_.markers.push_back(id_marker_);
00145 }
00146 this->tracksMarkers_publisher_.publish(this->MarkerArray_msg_);
00147 }
00148
00149
00150
00151
00152
00153
00154
00155 this->tracks_publisher_.publish(this->peopleTrackingArray_msg_);
00156 }
00157
00158
00159 void PeopleSimulationAlgNode::robot_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
00160 {
00161 ROS_INFO("PeopleSimulationAlgNode::robot_pose_callback: New Message Received");
00162
00163
00164 this->alg_.lock();
00165
00166
00167 scene_.update_robot( SdetectionObservation( 0, msg->header.stamp.toSec(),
00168 msg->pose.position.x, msg->pose.position.y ,
00169 0.0, 0.0 ) );
00170
00171
00172
00173 this->alg_.unlock();
00174
00175 }
00176
00177
00178 bool PeopleSimulationAlgNode::resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00179 {
00180 ROS_INFO("PeopleSimulationAlgNode::resetCallback: New Request Received!");
00181
00182
00183
00184
00185 if( simulation_mode_ == PeopleSimulationAlgNode::Density_incremental )
00186 {
00187
00188
00189
00190 }
00191
00192
00193 this->alg_.lock();
00194 ROS_INFO("PeopleSimulationAlgNode::resetCallback: Processin New Request!");
00195
00196 scene_.clear_scene();
00197 scene_.set_number_virtual_people( n_persons_ );
00198 this->alg_.unlock();
00199
00200
00201
00202
00203 return true;
00204 }
00205
00206
00207
00208
00209
00210 void PeopleSimulationAlgNode::node_config_update(Config &config, uint32_t level)
00211 {
00212 this->alg_.lock();
00213
00214 ROS_INFO(" ******* algorithm config update *******\n\n");
00215 simulation_mode_ = (PeopleSimulationAlgNode::simulation_mode) config.simulation_mode;
00216 vis_mode_ = config.vis_mode;
00217 n_persons_ = config.number_persons;
00218 scene_.set_number_virtual_people( n_persons_ );
00219
00220
00221 this->alg_.unlock();
00222 }
00223
00224 void PeopleSimulationAlgNode::addNodeDiagnostics(void)
00225 {
00226 }
00227
00228
00229 int main(int argc,char *argv[])
00230 {
00231 return algorithm_base::main<PeopleSimulationAlgNode>(argc, argv, "people_simulation_alg_node");
00232 }