00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00039 #include "ros/ros.h" 00040 #include "std_msgs/String.h" 00041 // #include "geometry_msgs/PoseWithCovariance.h" 00042 #include "tf/transform_listener.h" 00043 #include "mtt/TargetList.h" 00044 // #include <visualization_msgs/Marker.h> 00045 00046 // static ros::Time start_time; 00047 ros::Time time_last_msg(0); 00048 ros::Duration time_elapsed; 00049 // ros::Duration duration_btw_msg; 00050 // ros::Publisher nfeatures_pub; 00051 // ros::Publisher marker_pub; 00052 00053 tf::TransformListener *p_listener; 00054 tf::StampedTransform transform; 00055 00056 geometry_msgs::Twist twist; 00057 geometry_msgs::Twist features; 00058 // geometry_msgs::PoseWithCovariance nfeatures; 00059 00060 // std::list<geometry_msgs::PoseWithCovariance> matlab_list; 00061 00062 double target_x, target_y; 00063 double target_vel, target_theta; 00064 double robot_x, robot_y; 00065 double robot_vel, robot_theta; 00066 double position_diff, heading_diff; 00067 double angle_to_robot; 00068 double velocity_diff; 00069 uint leader_tag = 0; 00070 uint target_id = 0; 00071 uint single_target = 0; 00072 // uint counter = 0; 00073 00074 void targetsCallback(const mtt::TargetList& list) 00075 { 00076 //will print information that should be stored in a file 00077 //file format: id, good/bad tag, time, pos x, pos y, vel, theta 00078 // position_diff, heading_diff, angle_to_robot, velocity_diff 00079 static ros::Time start_time = ros::Time::now(); 00080 time_elapsed = ros::Time::now() - start_time; 00081 // matlab_list.clear(); 00082 00084 //use transformations to extract robot features 00085 try{ 00086 p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform); 00087 p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist); 00088 } 00089 catch (tf::TransformException ex){ 00090 ROS_ERROR("%s",ex.what()); 00091 } 00092 00093 robot_x = transform.getOrigin().x(); 00094 robot_y = transform.getOrigin().y(); 00095 robot_theta = tf::getYaw(transform.getRotation()); 00096 robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2)); 00097 00098 //robot output line 00100 printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,0,0,0,0\n", 00101 -1, leader_tag, time_elapsed.toSec(), 00102 robot_x, robot_y, robot_vel, robot_theta); 00103 00105 //sweeps target list and extract features 00106 for(uint i = 0; i < list.Targets.size(); i++){ 00107 target_id = list.Targets[i].id; 00108 target_x = list.Targets[i].pose.position.x; 00109 target_y = list.Targets[i].pose.position.y; 00110 target_theta = tf::getYaw(list.Targets[i].pose.orientation); 00111 target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+ 00112 pow(list.Targets[i].velocity.linear.y,2)); 00113 position_diff = sqrt(pow(robot_x - target_x,2)+ 00114 pow(robot_y - target_y,2)); 00115 heading_diff = robot_theta - target_theta; 00116 angle_to_robot = -robot_theta + atan2(target_y - robot_y, 00117 target_x - robot_x ); 00118 velocity_diff = robot_vel - target_vel; 00119 00120 //target output (to be used in adaboost training) 00121 00122 // % output file format: 00123 // % 1: id 00124 // % 2: good/bad tag 00125 // % 3: time 00126 // % 4: pos x 00127 // % 5: pos y 00128 // % 6: vel 00129 // % 7: theta 00130 // % 8: pos diff 00131 // % 9: head diff 00132 // %10: angle 2 robot 00133 // %11: velocity diff 00134 00136 printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n", 00137 target_id, leader_tag, time_elapsed.toSec(), 00138 target_x, target_y, target_vel, target_theta, 00139 position_diff, heading_diff, angle_to_robot, velocity_diff); 00140 00141 // if(position_diff < 6.0 && target_vel > 0.5){ 00142 // //if inside boundaries (in meters) 00143 // //store features in a covariance struct to send to matlab 00144 // nfeatures.pose.position.x = target_x; 00145 // nfeatures.pose.position.y = target_y; 00146 // nfeatures.pose.position.z = target_id; 00147 // 00148 // nfeatures.covariance[0] = target_vel; 00149 // nfeatures.covariance[1] = velocity_diff; 00150 // nfeatures.covariance[2] = heading_diff; 00151 // nfeatures.covariance[3] = angle_to_robot; 00152 // nfeatures.covariance[4] = position_diff; 00153 // 00154 // matlab_list.push_back(nfeatures); 00155 // // counter++; 00156 // } 00157 } 00158 // printf("targets within range: %d\n",counter); 00159 // counter = 0; 00160 00161 //check if enough time has passed 00162 //and send batch of msgs to matlab 00163 // duration_btw_msg = ros::Time::now() - time_last_msg; 00164 // 00165 // if(duration_btw_msg.toSec() > 0.01){ 00166 // while(!matlab_list.empty()){ 00167 // nfeatures_pub.publish(matlab_list.front()); 00168 // matlab_list.pop_front(); 00169 // usleep(0.01e6); //has to sleep, otherwise matlab do not get the msg 00170 // } 00171 // time_last_msg = ros::Time::now(); 00172 // } 00173 } 00174 00175 void tagCallback(const std_msgs::Header& tag) 00176 { 00177 //ROS_INFO("tag received"); 00178 //only works for a transition good/bad leader 00179 leader_tag = 1; 00180 } 00181 00182 // void drawCallback(const geometry_msgs::Pose& input) 00183 // { 00184 // visualization_msgs::Marker marker; 00185 // 00186 // marker.header.frame_id = "/map"; 00187 // marker.header.stamp = ros::Time::now(); 00188 // 00189 // marker.ns = "quality"; 00190 // marker.id = 0; 00191 // 00192 // marker.type = visualization_msgs::Marker::CYLINDER; 00193 // marker.action = visualization_msgs::Marker::ADD; 00194 // 00195 // // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 00196 // marker.pose.position.x = input.position.x; 00197 // marker.pose.position.y = input.position.y; 00198 // marker.pose.position.z = 0; 00199 // marker.pose.orientation.x = 0.0; 00200 // marker.pose.orientation.y = 0.0; 00201 // marker.pose.orientation.z = 0.0; 00202 // marker.pose.orientation.w = 1.0; 00203 // 00204 // // Set the scale of the marker 00205 // marker.scale.x = 0.5; 00206 // marker.scale.y = 0.5; 00207 // marker.scale.z = 0.5; 00208 // 00209 // //good or bad leader 00210 // if(input.position.z == -1){ 00211 // marker.color.r = 0.0f; 00212 // marker.color.g = 1.0f; 00213 // } 00214 // else if (input.position.z == 1){ 00215 // marker.color.r = 1.0f; 00216 // marker.color.g = 0.0f; 00217 // } 00218 // 00219 // marker.color.b = 0.0f; 00220 // marker.color.a = 1.0; 00221 // 00222 // marker.lifetime = ros::Duration(); 00223 // 00224 // // Publish the marker 00225 // marker_pub.publish(marker); 00226 // } 00227 00228 int main(int argc, char **argv) 00229 { 00230 ros::init(argc, argv, "process_target"); 00231 ros::NodeHandle n; 00232 00233 tf::TransformListener listener; 00234 p_listener=&listener; 00235 00236 //for testing purposes 00237 // single_target = atoi(argv[1]); 00238 // ROS_INFO("chosen target:%d",single_target); 00239 00240 ros::Subscriber sub = n.subscribe("/targets", 1000, targetsCallback); 00241 ros::Subscriber sub_tag = n.subscribe("/timetag", 1000, tagCallback); 00242 // ros::Subscriber sub_draw = n.subscribe("/pose_to_draw", 1000, drawCallback); 00243 // nfeatures_pub = n.advertise<geometry_msgs::PoseWithCovariance>("/example_topic", 100); 00244 // marker_pub = n.advertise<visualization_msgs::Marker>("/leader_quality", 1); 00245 00246 ros::spin(); 00247 00248 return 0; 00249 }