Merge pull request #4 from TeamSOBITS/noetic-check

Noetic devel (ros-perception#89)
TeamSOBITS · Jun 2, 2024 · 7e35369 · 7e35369
2 parents db8a2d8 + 1b2dbc6
commit 7e35369
Show file tree

Hide file tree

Showing 14 changed files with 97 additions and 167 deletions.
diff --git a/.cmake-format.yaml b/.cmake-format.yaml
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
diff --git a/README.en.md b/README.en.md
@@ -1,7 +1,7 @@
 # **ar_to_distance**
 <a name="readme-top"></a>
 
-[JP](README.md) | [EN](READMEen.md)
+[JP](README.md) | [EN](README_en.md)
 
 [![Contributors][contributors-shield]][contributors-url]
 [![Forks][forks-shield]][forks-url]

diff --git a/README.md b/README.md
@@ -1,7 +1,7 @@
 # **ar_to_distance**
 <a name="readme-top"></a>
 
-[JP](README.md) | [EN](README.en.md)
+[JP](README.md) | [EN](README_en.md)
 
 [![Contributors][contributors-shield]][contributors-url]
 [![Forks][forks-shield]][forks-url]

diff --git a/ar_track_alvar/CMakeLists.txt b/ar_track_alvar/CMakeLists.txt
@@ -143,18 +143,4 @@ if(CATKIN_ENABLE_TESTING)
   foreach(LAUNCH_FILE ${LAUNCH_FILES})
     roslaunch_add_file_check(${LAUNCH_FILE} USE_TEST_DEPENDENCIES)
   endforeach()
-
-  #catkin_download_test_data(
-  #  ${PROJECT_NAME}_4markers_tork.bag
-  #  http://download.ros.org/data/ar_track_alvar/ar_track_alvar_4markers_tork_2017-02-08-11-21-14.bag
-  #  # Workaround the issue http://answers.ros.org/question/253787/accessing-data-downloaded-via-catkin_download_test_data/
-  #  # by downloading into source folder.
-  #  #DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-  #  DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-  #  MD5 627aa0316bbfe4334e06023d7c2b4087
-  # )
-  #add_rostest(test/marker_arg_config-basic.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
-  #add_rostest(test/marker_arg_config-full.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
-  #add_rostest(test/marker_param_config-basic.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
-  #add_rostest(test/marker_param_config-full.test DEPENDENCIES ${PROJECT_NAME}_4markers_tork.bag)
 endif()
diff --git a/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h b/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h
@@ -125,8 +125,16 @@ class ALVAR_EXPORT LabelingCvSeq : public Labeling
   LabelingCvSeq();
   ~LabelingCvSeq();
 
+  /**
+   * \brief Unused function, functionality was left as a TODO
+   */
   void SetOptions(bool _detect_pose_grayscale = false);
 
+  /**
+   * \brief Labels image and filters blobs to obtain square-shaped objects from
+   * the scene. visualize parameter controls whether or not to draw the detected 
+   * borders of the marker
+   */
   void LabelSquares(cv::Mat& image, bool visualize = false);
 
   std::vector<std::vector<cv::Point>> LabelImage(cv::Mat& image, int min_size,

diff --git a/ar_track_alvar/include/ar_track_alvar/EC.h b/ar_track_alvar/include/ar_track_alvar/EC.h
@@ -325,112 +325,6 @@ class TrackerFeaturesEC : public TrackerFeatures
                  first_id, last_id);
   }
 
-  /** \brief Track features matching the given functor. New features will have
-   * id's in the specified id range. */
-  /*template<typename T, typename F>
-  bool Track(IplImage *img, IplImage *mask, std::map<int,T> &container, F
-  do_handle_test, int type_id=0, int first_id=0, int last_id=65535)
-  {
-    // Update features to match the ones in the given container
-    typename std::map<int,T>::iterator iter = container.begin();
-    typename std::map<int,T>::iterator iter_end = container.end();
-    feature_count = 0;
-    for (;iter != iter_end; iter++) {
-      T &f = iter->second;
-      if (!do_handle_test(f)) continue;
-      if (f.has_p2d != true) continue;
-      f.has_p2d = false; // This is updated again to true if tracking succeeds
-      features[feature_count] = f.p2d;
-      ids[feature_count] = iter->first;
-      feature_count++;
-      if (feature_count == max_features) break;
-    }
-    // Check that next_id is ok
-    if (next_id < first_id)	next_id = first_id;
-    if (next_id > last_id) return false; // TODO: Make some better solution for
-  this
-    // Purge if needed
-    if (purge) {
-      TrackerFeatures::Purge();
-      purge=false;
-    }
-    // Track as usual (this will swap above features to prev_features)
-    TrackHid(img, mask);
-    // Update the container to have the updated features
-    for (int i=0; i<feature_count; i++) {
-      int id = ids[i];
-      if (id >= last_id) break;
-      T &f = container[id];
-      f.type_id = type_id;
-      f.has_p2d = true;
-      f.p2d = features[i];
-    }
-    return true;
-  }*/
-
-  /** \brief Track features matching the given functor. If first_id >= 0 we call
-   * \e AddFeatures with the specified id range. */
-  template <typename T, typename F>
-  bool Track(IplImage* img, IplImage* mask, std::map<int, T>& container,
-             F do_handle_test, int type_id = 0, int first_id = -1,
-             int last_id = -1)
-  {
-    // When first_id && last_id are < 0 then we don't add new features...
-    if (first_id < 0)
-      last_id = -1;
-    // Update features to match the ones in the given container
-    typename std::map<int, T>::iterator iter = container.begin();
-    typename std::map<int, T>::iterator iter_end = container.end();
-    feature_count = 0;
-    for (; iter != iter_end; iter++)
-    {
-      T& f = iter->second;
-      if (!do_handle_test(f))
-        continue;
-      if (f.has_p2d != true)
-        continue;
-      f.has_p2d = false;  // This is updated again to true if tracking succeeds
-      features[feature_count] = f.p2d;
-      ids[feature_count] = iter->first;
-      feature_count++;
-      if (feature_count == max_features)
-        break;
-    }
-    // Purge if needed
-    if (purge)
-    {
-      TrackerFeatures::Purge();
-      purge = false;
-    }
-    if (first_id < 0)
-    {
-      // Track as usual (this will swap above features to prev_features)
-      TrackHid(img, mask, false);
-    }
-    else
-    {
-      // Check that next_id is ok
-      if (next_id < first_id)
-        next_id = first_id;
-      if (next_id > last_id)
-        return false;  // TODO: Make some better solution for this
-      // Track as usual (this will swap above features to prev_features)
-      TrackHid(img, mask, true);
-    }
-    // Update the container to have the updated features
-    for (int i = 0; i < feature_count; i++)
-    {
-      int id = ids[i];
-      if (last_id >= 0 && id >= last_id)
-        break;
-      T& f = container[id];
-      f.type_id = type_id;
-      f.has_p2d = true;
-      f.p2d = features[i];
-    }
-    return true;
-  }
-
   /** \brief add features to the previously tracked frame if there are less than
    * min_features */
   template <typename T>

diff --git a/ar_track_alvar/launch/pr2_bundle.launch b/ar_track_alvar/launch/pr2_bundle.launch
@@ -0,0 +1,14 @@
+<launch>
+	<arg name="marker_size" default="4.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.0" />
+
+    <arg name="cam_image_topic" default="/kinect_head/depth_registered/points" />
+	<arg name="cam_info_topic" default="/kinect_head/rgb/camera_info" />	
+
+	<arg name="output_frame" default="/torso_lift_link" />
+    <arg name="med_filt_size" default="10" />
+	<arg name="bundle_files" default="$(find ar_track_alvar)/bundles/truthTableLeg.xml $(find ar_track_alvar)/bundles/table_8_9_10.xml" />
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundles" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame) $(arg med_filt_size) $(arg bundle_files)" />
+</launch>
diff --git a/ar_track_alvar/launch/pr2_bundle_no_kinect.launch b/ar_track_alvar/launch/pr2_bundle_no_kinect.launch
@@ -0,0 +1,22 @@
+<launch>
+	<arg name="marker_size" default="4.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.2" />
+
+	<arg name="cam_image_topic" default="/wide_stereo/left/image_color" />
+	<arg name="cam_info_topic" default="/wide_stereo/left/camera_info" />
+
+	<arg name="output_frame" default="/torso_lift_link" />
+	<arg name="bundle_files" default="$(find ar_track_alvar)/bundles/truthTableLeg.xml $(find ar_track_alvar)/bundles/table_8_9_10.xml" />
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundlesNoKinect" respawn="false" output="screen">
+		<param name="marker_size"           type="double" value="$(arg marker_size)" />
+		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
+		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
+		<param name="output_frame"          type="string" value="$(arg output_frame)" />
+		<param name="bundle_files"          type="string" value="$(arg bundle_files)" />
+
+		<remap from="camera_image"  to="$(arg cam_image_topic)" />
+		<remap from="camera_info"   to="$(arg cam_info_topic)" />
+	</node>
+</launch>
diff --git a/ar_track_alvar/launch/pr2_indiv.launch b/ar_track_alvar/launch/pr2_indiv.launch
@@ -0,0 +1,20 @@
+<launch>
+	<arg name="marker_size" default="4.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.2" />
+
+	<arg name="cam_image_topic" default="/kinect_head/depth_registered/points" />
+	<arg name="cam_info_topic" default="/kinect_head/rgb/camera_info" />
+	<arg name="output_frame" default="/torso_lift_link" />
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
+		<param name="marker_size"           type="double" value="$(arg marker_size)" />
+		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
+		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
+		<param name="output_frame"          type="string" value="$(arg output_frame)" />
+
+		<remap from="camera_image"  to="$(arg cam_image_topic)" />
+		<remap from="camera_info"   to="$(arg cam_info_topic)" />
+	</node>
+
+</launch>
diff --git a/ar_track_alvar/launch/pr2_indiv_no_kinect.launch b/ar_track_alvar/launch/pr2_indiv_no_kinect.launch
@@ -0,0 +1,18 @@
+<launch>
+	<arg name="marker_size" default="4.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.2" />
+	<arg name="cam_image_topic" default="/wide_stereo/left/image_color" />
+	<arg name="cam_info_topic" default="/wide_stereo/left/camera_info" />
+	<arg name="output_frame" default="/torso_lift_link" />
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
+		<param name="marker_size"           type="double" value="$(arg marker_size)" />
+		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
+		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
+		<param name="output_frame"          type="string" value="$(arg output_frame)" />
+
+		<remap from="camera_image"  to="$(arg cam_image_topic)" />
+		<remap from="camera_info"   to="$(arg cam_info_topic)" />
+	</node>
+</launch>
diff --git a/ar_track_alvar/launch/pr2_train.launch b/ar_track_alvar/launch/pr2_train.launch
@@ -0,0 +1,11 @@
+<launch>
+	<arg name="nof_markers" default="8" />
+	<arg name="marker_size" default="4.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.2" />
+	<arg name="cam_image_topic" default="/wide_stereo/left/image_color" />
+	<arg name="cam_info_topic" default="/wide_stereo/left/camera_info" />	
+	<arg name="output_frame" default="/torso_lift_link" />
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="trainMarkerBundle" respawn="false" output="screen" args="$(arg nof_markers) $(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
+</launch>
diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp
@@ -64,7 +64,7 @@ MarkerDetector<MarkerData> marker_detector;
 
 bool enableSwitched = false;
 bool enabled = true;
-double max_frequency = 8.0;
+double max_frequency = 100.0;
 double marker_size;
 double max_new_marker_error;
 double max_track_error;

diff --git a/ar_track_alvar/nodes/TrainMarkerBundle.cpp b/ar_track_alvar/nodes/TrainMarkerBundle.cpp
@@ -81,7 +81,7 @@ std::string cam_info_topic;
 std::string output_frame;
 int nof_markers;
 
-double GetMultiMarkerPose(IplImage* image, Pose& pose);
+double GetMultiMarkerPose(cv::Mat& image, Pose& pose);
 void getCapCallback(const sensor_msgs::ImageConstPtr& image_msg);
 int keyCallback(int key);
 void makeMarkerMsgs(int type, int id, Pose& p,