sick_ldmrs_tools: sick_ldmrs_object

Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2016, DFKI GmbH
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of DFKI GmbH nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *      Authors:
00030  *         Martin Günther <martin.guenther@dfki.de>
00031  *         Jochen Sprickerhof <ros@jochen.sprickerhof.de>
00032  *
00033  */
00034 
00035 #include <ros/ros.h>
00036 
00037 #include <visualization_msgs/MarkerArray.h>
00038 #include <sick_ldmrs_msgs/ObjectArray.h>
00039 
00040 static const unsigned char GLASBEY_LUT[] =
00041 {
00042   255, 255, 255,
00043   0, 0, 255,
00044   255, 0, 0,
00045   0, 255, 0,
00046   0, 0, 51,
00047   255, 0, 182,
00048   0, 83, 0,
00049   255, 211, 0,
00050   0, 159, 255,
00051   154, 77, 66,
00052   0, 255, 190,
00053   120, 63, 193,
00054   31, 150, 152,
00055   255, 172, 253,
00056   177, 204, 113,
00057   241, 8, 92,
00058   254, 143, 66,
00059   221, 0, 255,
00060   32, 26, 1,
00061   114, 0, 85,
00062   118, 108, 149,
00063   2, 173, 36,
00064   200, 255, 0,
00065   136, 108, 0,
00066   255, 183, 159,
00067   133, 133, 103,
00068   161, 3, 0,
00069   20, 249, 255,
00070   0, 71, 158,
00071   220, 94, 147,
00072   147, 212, 255,
00073   0, 76, 255,
00074   0, 66, 80,
00075   57, 167, 106,
00076   238, 112, 254,
00077   0, 0, 100,
00078   171, 245, 204,
00079   161, 146, 255,
00080   164, 255, 115,
00081   255, 206, 113,
00082   71, 0, 21,
00083   212, 173, 197,
00084   251, 118, 111,
00085   171, 188, 0,
00086   117, 0, 215,
00087   166, 0, 154,
00088   0, 115, 254,
00089   165, 93, 174,
00090   98, 132, 2,
00091   0, 121, 168,
00092   0, 255, 131,
00093   86, 53, 0,
00094   159, 0, 63,
00095   66, 45, 66,
00096   255, 242, 187,
00097   0, 93, 67,
00098   252, 255, 124,
00099   159, 191, 186,
00100   167, 84, 19,
00101   74, 39, 108,
00102   0, 16, 166,
00103   145, 78, 109,
00104   207, 149, 0,
00105   195, 187, 255,
00106   253, 68, 64,
00107   66, 78, 32,
00108   106, 1, 0,
00109   181, 131, 84,
00110   132, 233, 147,
00111   96, 217, 0,
00112   255, 111, 211,
00113   102, 75, 63,
00114   254, 100, 0,
00115   228, 3, 127,
00116   17, 199, 174,
00117   210, 129, 139,
00118   91, 118, 124,
00119   32, 59, 106,
00120   180, 84, 255,
00121   226, 8, 210,
00122   0, 1, 20,
00123   93, 132, 68,
00124   166, 250, 255,
00125   97, 123, 201,
00126   98, 0, 122,
00127   126, 190, 58,
00128   0, 60, 183,
00129   255, 253, 0,
00130   7, 197, 226,
00131   180, 167, 57,
00132   148, 186, 138,
00133   204, 187, 160,
00134   55, 0, 49,
00135   0, 40, 1,
00136   150, 122, 129,
00137   39, 136, 38,
00138   206, 130, 180,
00139   150, 164, 196,
00140   180, 32, 128,
00141   110, 86, 180,
00142   147, 0, 185,
00143   199, 48, 61,
00144   115, 102, 255,
00145   15, 187, 253,
00146   172, 164, 100,
00147   182, 117, 250,
00148   216, 220, 254,
00149   87, 141, 113,
00150   216, 85, 34,
00151   0, 196, 103,
00152   243, 165, 105,
00153   216, 255, 182,
00154   1, 24, 219,
00155   52, 66, 54,
00156   255, 154, 0,
00157   87, 95, 1,
00158   198, 241, 79,
00159   255, 95, 133,
00160   123, 172, 240,
00161   120, 100, 49,
00162   162, 133, 204,
00163   105, 255, 220,
00164   198, 82, 100,
00165   121, 26, 64,
00166   0, 238, 70,
00167   231, 207, 69,
00168   217, 128, 233,
00169   255, 211, 209,
00170   209, 255, 141,
00171   36, 0, 3,
00172   87, 163, 193,
00173   211, 231, 201,
00174   203, 111, 79,
00175   62, 24, 0,
00176   0, 117, 223,
00177   112, 176, 88,
00178   209, 24, 0,
00179   0, 30, 107,
00180   105, 200, 197,
00181   255, 203, 255,
00182   233, 194, 137,
00183   191, 129, 46,
00184   69, 42, 145,
00185   171, 76, 194,
00186   14, 117, 61,
00187   0, 30, 25,
00188   118, 73, 127,
00189   255, 169, 200,
00190   94, 55, 217,
00191   238, 230, 138,
00192   159, 54, 33,
00193   80, 0, 148,
00194   189, 144, 128,
00195   0, 109, 126,
00196   88, 223, 96,
00197   71, 80, 103,
00198   1, 93, 159,
00199   99, 48, 60,
00200   2, 206, 148,
00201   139, 83, 37,
00202   171, 0, 255,
00203   141, 42, 135,
00204   85, 83, 148,
00205   150, 255, 0,
00206   0, 152, 123,
00207   255, 138, 203,
00208   222, 69, 200,
00209   107, 109, 230,
00210   30, 0, 68,
00211   173, 76, 138,
00212   255, 134, 161,
00213   0, 35, 60,
00214   138, 205, 0,
00215   111, 202, 157,
00216   225, 75, 253,
00217   255, 176, 77,
00218   229, 232, 57,
00219   114, 16, 255,
00220   111, 82, 101,
00221   134, 137, 48,
00222   99, 38, 80,
00223   105, 38, 32,
00224   200, 110, 0,
00225   209, 164, 255,
00226   198, 210, 86,
00227   79, 103, 77,
00228   174, 165, 166,
00229   170, 45, 101,
00230   199, 81, 175,
00231   255, 89, 172,
00232   146, 102, 78,
00233   102, 134, 184,
00234   111, 152, 255,
00235   92, 255, 159,
00236   172, 137, 178,
00237   210, 34, 98,
00238   199, 207, 147,
00239   255, 185, 30,
00240   250, 148, 141,
00241   49, 34, 78,
00242   254, 81, 97,
00243   254, 141, 100,
00244   68, 54, 23,
00245   201, 162, 84,
00246   199, 232, 240,
00247   68, 152, 0,
00248   147, 172, 58,
00249   22, 75, 28,
00250   8, 84, 121,
00251   116, 45, 0,
00252   104, 60, 255,
00253   64, 41, 38,
00254   164, 113, 215,
00255   207, 0, 155,
00256   118, 1, 35,
00257   83, 0, 88,
00258   0, 82, 232,
00259   43, 92, 87,
00260   160, 217, 146,
00261   176, 26, 229,
00262   29, 3, 36,
00263   122, 58, 159,
00264   214, 209, 207,
00265   160, 100, 105,
00266   106, 157, 160,
00267   153, 219, 113,
00268   192, 56, 207,
00269   125, 255, 89,
00270   149, 0, 34,
00271   213, 162, 223,
00272   22, 131, 204,
00273   166, 249, 69,
00274   109, 105, 97,
00275   86, 188, 78,
00276   255, 109, 81,
00277   255, 3, 248,
00278   255, 0, 73,
00279   202, 0, 35,
00280   67, 109, 18,
00281   234, 170, 173,
00282   191, 165, 0,
00283   38, 44, 51,
00284   85, 185, 2,
00285   121, 182, 158,
00286   254, 236, 212,
00287   139, 165, 89,
00288   141, 254, 193,
00289   0, 60, 43,
00290   63, 17, 40,
00291   255, 221, 246,
00292   17, 26, 146,
00293   154, 66, 84,
00294   149, 157, 238,
00295   126, 130, 72,
00296   58, 6, 101,
00297   189, 117, 101,
00298 };
00299 
00300 ros::Publisher pub;
00301 
00302 void callback(const sick_ldmrs_msgs::ObjectArray::ConstPtr& oa)
00303 {
00304   visualization_msgs::MarkerArray velocity;
00305   velocity.markers.resize(oa->objects.size());
00306 
00307   for (size_t i = 0; i < oa->objects.size(); i++)
00308   {
00309     velocity.markers[i].header = oa->header;
00310     velocity.markers[i].ns = "velocities";
00311     velocity.markers[i].id = oa->objects[i].id;
00312     velocity.markers[i].type = visualization_msgs::Marker::ARROW;
00313     velocity.markers[i].action = visualization_msgs::Marker::ADD;
00314     velocity.markers[i].scale.x = 0.1;
00315     velocity.markers[i].scale.y = 0.2;
00316     velocity.markers[i].color.a = 0.75;
00317     velocity.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
00318     velocity.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
00319     velocity.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
00320     velocity.markers[i].lifetime = ros::Duration(0.5);
00321 
00322     velocity.markers[i].points.resize(2);
00323     velocity.markers[i].points[0] = oa->objects[i].object_box_center.pose.position;
00324     velocity.markers[i].points[1].x = oa->objects[i].object_box_center.pose.position.x + oa->objects[i].velocity.twist.linear.x;
00325     velocity.markers[i].points[1].y = oa->objects[i].object_box_center.pose.position.y + oa->objects[i].velocity.twist.linear.y;
00326   }
00327 
00328   pub.publish(velocity);
00329 
00330   visualization_msgs::MarkerArray bounding_box;
00331   bounding_box.markers.resize(oa->objects.size());
00332 
00333   for (size_t i = 0; i < oa->objects.size(); i++)
00334   {
00335     bounding_box.markers[i].header = oa->header;
00336     bounding_box.markers[i].ns = "bounding_boxes";
00337     bounding_box.markers[i].id = oa->objects[i].id;
00338     bounding_box.markers[i].type = visualization_msgs::Marker::CUBE;
00339     bounding_box.markers[i].action = visualization_msgs::Marker::ADD;
00340     bounding_box.markers[i].color.a = 0.75;
00341     bounding_box.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
00342     bounding_box.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
00343     bounding_box.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
00344     bounding_box.markers[i].lifetime = ros::Duration(0.5);
00345 
00346     bounding_box.markers[i].pose = oa->objects[i].bounding_box_center;
00347     bounding_box.markers[i].scale = oa->objects[i].bounding_box_size;
00348 
00349     if (bounding_box.markers[i].scale.x == 0.0)
00350     {
00351       bounding_box.markers[i].scale.x = 0.01;
00352     }
00353     if (bounding_box.markers[i].scale.y == 0.0)
00354     {
00355       bounding_box.markers[i].scale.y = 0.01;
00356     }
00357     bounding_box.markers[i].scale.z = 0.2;
00358   }
00359 
00360   pub.publish(bounding_box);
00361 
00362   visualization_msgs::MarkerArray object_boxes;
00363   object_boxes.markers.resize(oa->objects.size());
00364 
00365   for (size_t i = 0; i < oa->objects.size(); i++)
00366   {
00367     object_boxes.markers[i].header = oa->header;
00368     object_boxes.markers[i].ns = "object_boxes";
00369     object_boxes.markers[i].id = oa->objects[i].id;
00370     object_boxes.markers[i].type = visualization_msgs::Marker::CUBE;
00371     object_boxes.markers[i].action = visualization_msgs::Marker::ADD;
00372     object_boxes.markers[i].color.a = 0.75;
00373     object_boxes.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
00374     object_boxes.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
00375     object_boxes.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
00376     object_boxes.markers[i].lifetime = ros::Duration(0.5);
00377 
00378     object_boxes.markers[i].pose = oa->objects[i].object_box_center.pose;
00379     object_boxes.markers[i].scale = oa->objects[i].object_box_size;
00380 
00381     if (object_boxes.markers[i].scale.x == 0.0)
00382     {
00383       object_boxes.markers[i].scale.x = 0.01;
00384     }
00385     if (object_boxes.markers[i].scale.y == 0.0)
00386     {
00387       object_boxes.markers[i].scale.y = 0.01;
00388     }
00389     object_boxes.markers[i].scale.z = 0.2;
00390   }
00391 
00392   pub.publish(object_boxes);
00393 
00394   visualization_msgs::MarkerArray contour_lines;
00395   contour_lines.markers.resize(oa->objects.size());
00396 
00397   for (size_t i = 0; i < oa->objects.size(); i++)
00398   {
00399     contour_lines.markers[i].header = oa->header;
00400     contour_lines.markers[i].ns = "contour_lines";
00401     contour_lines.markers[i].id = oa->objects[i].id;
00402     contour_lines.markers[i].type = visualization_msgs::Marker::LINE_STRIP;
00403     contour_lines.markers[i].action = visualization_msgs::Marker::ADD;
00404     contour_lines.markers[i].scale.x = 0.1;
00405     contour_lines.markers[i].color.a = 0.75;
00406     contour_lines.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
00407     contour_lines.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
00408     contour_lines.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
00409     contour_lines.markers[i].lifetime = ros::Duration(0.5);
00410 
00411     contour_lines.markers[i].points = oa->objects[i].contour_points;
00412   }
00413 
00414   pub.publish(contour_lines);
00415 }
00416 
00417 
00418 int main(int argc, char **argv)
00419 {
00420   ros::init(argc, argv, "sick_ldmrs_object_marker");
00421   ros::NodeHandle nh;
00422 
00423   ros::Subscriber sub = nh.subscribe("objects", 1, callback);
00424   pub = nh.advertise<visualization_msgs::MarkerArray>("object_markers", 1);
00425 
00426   ros::spin();
00427 
00428   return 0;
00429 }