Merge pull request #4 from pet1330/to-fix

combined messages

Author: marc-hanheide

CMakeLists.txt

@@ -35,10 +35,15 @@ if(NOT DISABLE_ROS)
   add_message_files(
     FILES
     PointArray.msg
+    Tracker.msg
+    TrackerArray.msg
   )
 
   generate_messages(
-    DEPENDENCIES geometry_msgs
+    DEPENDENCIES 
+    geometry_msgs
+    std_msgs
+    whycon
   )
 
   catkin_package(

launch/whycon.launch

@@ -1,19 +1,24 @@
+<?xml version="1.0"?>
 <launch>
-  <group ns="camera">
-    <node pkg="image_proc" type="image_proc" name="image_proc"/>
-  </group>
-
-   <node name="whycon" type="whycon" pkg="whycon">
-    <param name="targets" value="4"/> 
-    <param name="max_refine" value="5"/>
-    <param name="max_attempts" value="5"/>
-
-    <!-- uncomment when already performed 3D->2D calibration -->
-    <!-- 
-      <param name="xscale" value="1"/>
-      <param name="yscale" value="1"/>
-      <param name="axis" value="axis/>
-    --> 
+  
+  <node pkg="whycon" type="whycon" name="whycon_right" respawn="true" output="screen">
+  <param name="targets" value="1" type="int"/>
+  <param name="outer_diameter" value="0.03" type="double"/>
+  <param name="inner_diameter" value="0.01" type="double"/>
+  <param name="target_frame" value="right_hand_camera"/>
+  <remap from="/camera/image_rect_color" to="/cameras/right_hand_camera/image"/>
+  <remap from="/camera/camera_info" to="/cameras/right_hand_camera/camera_info"/>
   </node>
-</launch>
 
+  <node pkg="whycon" type="whycon" name="whycon_left" respawn="true" output="screen">
+  <param name="targets" value="1" type="int"/>
+  <param name="outer_diameter" value="0.03" type="double"/>
+  <param name="inner_diameter" value="0.01" type="double"/>
+  <param name="target_frame" value="left_hand_camera"/>
+  <remap from="/camera/image_rect_color" to="/cameras/left_hand_camera/image"/>
+  <remap from="/camera/camera_info" to="/cameras/left_hand_camera/camera_info"/>
+  </node>
+
+  <!--<node pkg="zoidbot_tools" type="whycon_transform.py" name="whycon_transform" respawn="true" output="screen"/>-->
+  
+</launch>

msg/Tracker.msg

@@ -0,0 +1,5 @@
+string uuid                         # A unique label for that object or a universal ID
+geometry_msgs/Pose pose             # The position and orientation of each object
+geometry_msgs/Point point 			# Image Coordinates
+float64 roundness                   # The circulatiry of the object
+float64 bwratio                     # The ratio between the inside and outside circle

msg/TrackerArray.msg

@@ -0,0 +1,2 @@
+Header header
+whycon/Tracker[] tracked_objects          # A list of detected objects from a single timestamp

src/lib/circle_detector.cpp

@@ -252,12 +252,13 @@ cv::CircleDetector::Circle cv::CircleDetector::detect(const cv::Mat& image, bool
   vector<int>& queue = context->queue;
 
 	int pos = (height-1)*width;
-  int ii = 0;
+	int ii = 0;
 	int start = 0;
   Circle inner, outer;
 
 	bool search_in_window = false;
-	int local_x, local_y;
+	int local_x = -1;
+	int local_y = -1;
 	if (previous_circle.valid){
 		WHYCON_DEBUG("starting with previously valid circle at " << previous_circle.x << "," << previous_circle.y);
 		ii = ((int)previous_circle.y)*width+(int)previous_circle.x;

src/lib/localization_system.cpp

@@ -8,9 +8,6 @@
 #include <limits>
 #include <whycon/circle_detector.h>
 #include <whycon/localization_system.h>
-using std::cout;
-using std::endl;
-using std::numeric_limits;
 
 cv::LocalizationSystem::LocalizationSystem(int _targets, int _width, int _height, const cv::Mat& _K, const cv::Mat& _dist_coeff, 
   float _outer_diameter, float _inner_diameter) :
@@ -31,7 +28,7 @@ cv::LocalizationSystem::LocalizationSystem(int _targets, int _width, int _height
   coordinates_transform = cv::Matx33f(1, 0, 0, 0, 1, 0, 0, 0, 1);
   precompute_undistort_map();
 
-  cout.precision(30);
+  std::cout.precision(30);
 }
 
 bool cv::LocalizationSystem::localize(const cv::Mat& image, bool reset, int attempts, int max_refine) {
@@ -159,21 +156,21 @@ bool cv::LocalizationSystem::set_axis(const cv::Mat& image, int max_attempts, in
   cv::Vec3f vecs[3];  
   for (int i = 0; i < 3; i++) {
     vecs[i] = circle_poses[i + 1].pos - circle_poses[0].pos;
-    cout << "vec " << i+1 << "->0 " << vecs[i] << endl;
+    std::cout << "vec " << i+1 << "->0 " << vecs[i] << std::endl;
   }
   int min_prod_i = 0;
-  float min_prod = numeric_limits<float>::max();
+  float min_prod = std::numeric_limits<float>::max();
   for (int i = 0; i < 3; i++) {
     float prod = fabsf(vecs[(i + 2) % 3].dot(vecs[i]));
-    cout << "prod: " << ((i + 2) % 3 + 1) << " " << i + 1 << " " << vecs[(i + 2) % 3] << " " << vecs[i] << " " << prod << endl;
+    std::cout << "prod: " << ((i + 2) % 3 + 1) << " " << i + 1 << " " << vecs[(i + 2) % 3] << " " << vecs[i] << " " << prod << std::endl;
     if (prod < min_prod) { min_prod = prod; min_prod_i = i; }
   }
   int axis1_i = (((min_prod_i + 2) % 3) + 1);
   int axis2_i = (min_prod_i + 1);
   if (fabsf(circle_poses[axis1_i].pos(0)) < fabsf(circle_poses[axis2_i].pos(0))) std::swap(axis1_i, axis2_i);
   int xy_i = 0;
   for (int i = 1; i <= 3; i++) if (i != axis1_i && i != axis2_i) { xy_i = i; break; }
-  cout << "axis ids: " << axis1_i << " " << axis2_i << " " << xy_i << endl;
+  std::cout << "axis ids: " << axis1_i << " " << axis2_i << " " << xy_i << std::endl;
 
   CircleDetector::Circle origin_circles_reordered[4];
   origin_circles_reordered[0] = origin_circles[0];
@@ -183,7 +180,7 @@ bool cv::LocalizationSystem::set_axis(const cv::Mat& image, int max_attempts, in
   for (int i = 0; i < 4; i++) {
     origin_circles[i] = origin_circles_reordered[i];
     circle_poses[i] = get_pose(origin_circles[i]);
-    cout << "original poses: " << circle_poses[i].pos << endl;
+    std::cout << "original poses: " << circle_poses[i].pos << std::endl;
   }
 
   float dim_x = 1.0;
@@ -208,14 +205,14 @@ bool cv::LocalizationSystem::set_axis(const cv::Mat& image, int max_attempts, in
   cv::solve(A, b, x);
   x.push_back(1.0);
   coordinates_transform = x.reshape(1, 3);
-  cout << "H " << coordinates_transform << endl;
+  std::cout << "H " << coordinates_transform << std::endl;
 
   // TODO: compare H obtained by OpenCV with the hand approach
   std::vector<cv::Vec2f> src(4), dsts(4);
   for (int i = 0; i < 4; i++) {
     src[i] = tmp[i];
     dsts[i] = targets[i];
-    cout << tmp[i] << " -> " << targets[i] << endl;
+    std::cout << tmp[i] << " -> " << targets[i] << std::endl;
   }
 
   /*cv::Matx33f H = cv::findHomography(src, dsts, CV_LMEDS);

src/ros/whycon_node.cpp

@@ -3,7 +3,7 @@
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "whycon");
+  ros::init(argc, argv, "whycon", ros::init_options::AnonymousName);
   ros::NodeHandle n("~");
 
   whycon::WhyConROS whycon_ros(n);