Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <nav_msgs/OccupancyGrid.h>
00003 #include <geometry_msgs/Twist.h>
00004 #include <nav_msgs/Odometry.h>
00005 #include <dynamic_reconfigure/server.h>
00006 #include <riskrrt/OccupancyGridArray.h>
00007 #include <riskrrt/PoseTwistStamped.h>
00008 #include <riskrrt/Trajectory.h>
00009 #include <vector>
00010 #include <iostream>
00011 #include <tf/tf.h>
00012 #include <algorithm>
00013 #include <visualization_msgs/MarkerArray.h>
00014
00015 using namespace std;
00016
00017 struct custom_pose{
00018 double x;
00019 double y;
00020 double theta;
00021 };
00022
00023 nav_msgs::OccupancyGrid grid;
00024
00025 riskrrt::OccupancyGridArray og_array;
00026 int nbMap;
00027 double timeStep;
00028 double arg_vel;
00029 int ped_num;
00030 custom_pose temp_pose, ped_pose;
00031 bool straight;
00032
00033 visualization_msgs::MarkerArray marker_array;
00034 visualization_msgs::Marker marker;
00035
00036
00037 int gridIFromPose(custom_pose pose){
00038 return (int)round((pose.x - grid.info.origin.position.x) / grid.info.resolution);
00039 }
00040
00041 int gridJFromPose(custom_pose pose){
00042 return (int)round((pose.y - grid.info.origin.position.y) / grid.info.resolution);
00043 }
00044
00045 int gridIndexFromCoord(int i, int j){
00046 return i + grid.info.width * j;
00047 }
00048
00049 int gridIFromIndex(int index){
00050 return index % grid.info.width;
00051 }
00052
00053 int gridJFromIndex(int index){
00054 return floor(index / grid.info.width);
00055 }
00056
00057 custom_pose poseFromGridCoord(int i, int j){
00058 custom_pose pose;
00059 pose.x = grid.info.resolution * i + grid.info.origin.position.x;
00060 pose.y = grid.info.resolution * j + grid.info.origin.position.y;
00061 return pose;
00062 }
00063
00064 int gridIndexFromPose(custom_pose pose){
00065 int index, i, j;
00066 i = gridIFromPose(pose);
00067 j = gridJFromPose(pose);
00068 index = gridIndexFromCoord(i, j);
00069 return index;
00070 }
00071
00072 custom_pose poseFromGridIndex(int index){
00073 int i, j;
00074 custom_pose pose;
00075
00076 i = gridIFromIndex(index);
00077 j = gridJFromIndex(index);
00078 pose = poseFromGridCoord(i, j);
00079
00080 return pose;
00081 }
00082
00083 void OGCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00084 {
00085 int i;
00086
00087 grid = *msg;
00088
00089 og_array.array.clear();
00090 for(i=0;i<nbMap;i++){
00091 og_array.array.push_back(grid);
00092 }
00093
00094 }
00095
00096 void pedposeCallback(const nav_msgs::Odometry::ConstPtr& msg)
00097 {
00098
00099 int i, j, k;
00100 int ped_grid_index;
00101 int ped_grid_i;
00102 int ped_grid_j;
00103
00104 int max_i, min_i;
00105 int max_j, min_j;
00106
00107 ped_pose.x = msg->pose.pose.position.x;
00108 ped_pose.y = msg->pose.pose.position.y;
00109 ped_pose.theta = tf::getYaw(msg->pose.pose.orientation);
00110
00111
00112 marker.header.frame_id = "/map";
00113 marker.header.stamp = ros::Time();
00114 marker.ns = "humans";
00115 marker.id = rand();
00116 marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00117 marker.action = visualization_msgs::Marker::ADD;
00118 marker.pose.position.x = ped_pose.x;
00119 marker.pose.position.y = ped_pose.y;
00120 marker.pose.position.z = 0;
00121
00122 if(straight){
00123 marker.pose.orientation = tf::createQuaternionMsgFromYaw(ped_pose.theta + 1.57);
00124 }
00125 else marker.pose.orientation = tf::createQuaternionMsgFromYaw(ped_pose.theta + 1.57 + M_PI);
00126 marker.scale.x = 0.4*0.0254;
00127 marker.scale.y = 0.4*0.0254;
00128 marker.scale.z = 0.4*0.0254;
00129
00130
00131
00132
00133 marker.lifetime = ros::Duration(0.125);
00134 marker.mesh_use_embedded_materials = true;
00135 marker.mesh_resource = "package://riskrrt/meshes/female2/models/female2.dae";
00136 marker_array.markers.push_back(marker);
00137
00138 for(k=0;k<nbMap;k++){
00139
00140 temp_pose.x = ped_pose.x + k*timeStep*arg_vel*cos(ped_pose.theta);
00141 temp_pose.y = ped_pose.y + k*timeStep*arg_vel*sin(ped_pose.theta);
00142 ped_grid_i = gridIFromPose(temp_pose);
00143 ped_grid_j = gridJFromPose(temp_pose);
00144
00145 min_i = max(ped_grid_i - 5, 0);
00146 max_i = min(ped_grid_i + 5, (int)grid.info.width);
00147 min_j = max(ped_grid_j - 5, 0);
00148 max_j = min(ped_grid_j + 5, (int)grid.info.height);
00149
00150 for(i=min_i ; i<=max_i ; i++){
00151 for(j=min_j ; j<=max_j ; j++){
00152 og_array.array[k].data[gridIndexFromCoord(i,j)] = 100;
00153 }
00154 }
00155 }
00156
00157 }
00158
00159 int main(int argc, char **argv)
00160 {
00161 ros::init(argc, argv, "og_builder_stage");
00162
00163 ros::NodeHandle n;
00164
00165 if (argc != 3){
00166 ROS_INFO("USAGE: og_builder_stage [number of pedestrians] [speed]");
00167 return 0;
00168 }
00169
00170 n.param("maxDepth", nbMap, 10);
00171 n.param("timeStep", timeStep, 0.5);
00172
00173 geometry_msgs::Twist ped_vel;
00174 arg_vel = atof(argv[2]);
00175 ped_num = atoi(argv[1]);
00176
00177 stringstream ss;
00178 string str;
00179 int i;
00180
00181 ros::Subscriber og_sub = n.subscribe("map", 1, OGCallback);
00182
00183 vector<ros::Subscriber> sub_vect;
00184 ros::Subscriber sub;
00185 for(i=0;i<ped_num;i++){
00186 ss << "/robot_" << i+1 << "/base_pose_ground_truth";
00187 str = ss.str();
00188 sub = n.subscribe(str, 10, pedposeCallback);
00189 sub_vect.push_back(sub);
00190 ss.clear();
00191 ss.str(string());
00192 }
00193
00194 ros::Publisher test_pub = n.advertise<nav_msgs::OccupancyGrid>("test_map", 1);
00195 ros::Publisher og_pub = n.advertise<riskrrt::OccupancyGridArray>("ogarray", 1);
00196
00197 vector<ros::Publisher> pub_vect;
00198 ros::Publisher pub;
00199 for(i=0;i<ped_num;i++){
00200 ss << "/robot_" << i+1 << "/cmd_vel";
00201 str = ss.str();
00202 pub = n.advertise<geometry_msgs::Twist>(str, 1);
00203 pub_vect.push_back(pub);
00204 ss.clear();
00205 ss.str(string());
00206 }
00207
00208 ros::Rate loop_rate(10);
00209 ros::Publisher vis_pub = n.advertise<visualization_msgs::MarkerArray>( "human_marker", 0 );
00210
00211 ros::Time begin;
00212 begin = ros::Time::now();
00213 ros::Time present;
00214 ros::Duration dur1(30.0);
00215 ros::Duration dur2(60.0);
00216 straight = true;
00217
00218 while (ros::ok())
00219 {
00220
00221 present = ros::Time::now();
00222 if(present-begin <= dur1){
00223 straight=true;
00224 }
00225 else if(present-begin <= dur2){
00226 straight=false;
00227 }
00228 else begin=ros::Time::now();
00229
00230 if(straight){
00231 arg_vel = fabs(arg_vel);
00232 ped_vel.linear.x = arg_vel;
00233 }
00234 else{
00235 arg_vel = -fabs(arg_vel);
00236 ped_vel.linear.x = arg_vel;
00237 }
00238
00239 marker_array.markers.clear();
00240
00241 ros::spinOnce();
00242
00243 og_pub.publish(og_array);
00244 test_pub.publish(og_array.array[nbMap-1]);
00245 og_array.array.clear();
00246 for(i=0;i<nbMap;i++){
00247 og_array.array.push_back(grid);
00248 }
00249
00250
00251 for(i=0;i<ped_num;i++){
00252 pub_vect[i].publish(ped_vel);
00253 }
00254
00255 vis_pub.publish(marker_array);
00256 marker_array.markers.clear();
00257
00258 loop_rate.sleep();
00259 }
00260 return 0;
00261 }
00262