Resolve merge conflict by incorporating both suggestions

.cmake-format.yaml

@@ -0,0 +1,33 @@
+additional_commands:
+  foo:
+    flags:
+    - BAR
+    - BAZ
+    kwargs:
+      DEPENDS: '*'
+      HEADERS: '*'
+      SOURCES: '*'
+algorithm_order:
+- 0
+- 1
+- 2
+- 3
+always_wrap: []
+bullet_char: '*'
+command_case: lower
+dangle_parens: false
+emit_byteorder_mark: false
+enable_markup: false
+enum_char: .
+fence_pattern: ^\s*([`~]{3}[`~]*)(.*)$
+first_comment_is_literal: false
+keyword_case: upper
+line_ending: unix
+line_width: 80
+literal_comment_pattern: null
+max_subargs_per_line: 3
+per_command: {}
+ruler_pattern: ^\s*[^\w\s]{3}.*[^\w\s]{3}$
+separate_ctrl_name_with_space: false
+separate_fn_name_with_space: false
+tab_size: 2

.pre-commit-config.yaml

@@ -0,0 +1,10 @@
+- repo: local
+  hooks:
+    -   id: clang-format
+        name: Run clang-format
+        description: Clang C/C++ code formatter.
+        entry: clang-format -i
+        args: [-style=file]
+        language: system
+        types: [file, c++]
+        # note: formatting style in .clang-format

CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ar_track_alvar
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.0 (202X-XX-XX)
+-------------------
+* first public release for ROS Noetic

README.en.md

@@ -0,0 +1,99 @@
+# **ar_to_distance**
+<a name="readme-top"></a>
+
+[JP](README.md) | [EN](README_en.md)
+
+[![Contributors][contributors-shield]][contributors-url]
+[![Forks][forks-shield]][forks-url]
+[![Stargazers][stars-shield]][stars-url]
+[![Issues][issues-shield]][issues-url]
+<!-- [![MIT License][license-shield]][license-url] -->
+
+<!-- Table of Contents -->
+<details>
+  <summary>Table of Contents</summary>
+  <ol>
+    <li>
+      <a href="#overview">Overview</a>
+    </li>
+    <li>
+      <a href="#setup">Setup</a>
+      <ul>
+        <li><a href="#environment">Environment</a></li>
+        <li><a href="#installation">Installation</a></li>
+      </ul>
+    </li>
+    <li><a href="#usage">Usage</a></li>
+    <li><a href="#milestones">Milestones</a></li>
+    <li><a href="#changelog">Changelog</a></li>
+    <!-- <li><a href="#contributing">Contributing</a></li> -->
+    <!-- <li><a href="#license">License</a></li> -->
+    <li><a href="#references">References</a></li>
+  </ol>
+</details>
+
+<!-- Overview -->
+## Overview
+- Package to calculate distance from AR codes
+- Sends TF relative to camera from AR codes.
+※ Please detect using the AR markers located in /ar_track_alvar/markers.
+<p align="right">(<a href="#readme-top">Back to top</a>)</p>
+
+<!-- Setup -->
+## Setup
+See [ROS wiki](http://wiki.ros.org/ar_track_alvar) for the user's documentation.
+
+### Environment
+Below are the operating environment details required for proper functioning.
+| System  | Version |
+| ------------- | ------------- |
+| Ubuntu    | 20.04 (Focal Fossa) |
+| ROS       | Noetic Ninjemys |
+
+<!-- Usage -->
+## Usage
+<!-- It would be helpful to have instructions on how to run the demo or include screenshots -->
+- Azure Kinect Launcher (Detection using Azure Kinect)
+```
+$ roslaunch ar_track_alvar azure_kinect.launch
+```
+※If using a different camera, please make sure to change the topic name and frame name accordingly.
+
+- [Back to Top](#ar_to_distance)
+
+<!-- Milestones -->
+## Milestones
+- [ ] Open Source Software
+    - [ ] Improve documentation
+    - [ ] Standardize coding style
+
+Check the [Issue page](https://github.com/ros-perception/ar_track_alvar/issues) for current bugs or feature requests.
+
+<p align="right">(<a href="#readme-top">Back to top</a>)</p>
+
+<!-- Changelog -->
+## Changelog
+  Please refer to the rst.file for the changelog.
+<!-- References -->
+## References
+
+* []()
+* []()
+* []()
+
+<p align="right">(<a href="#readme-top">Back to top</a>)</p>
+
+
+
+<!-- MARKDOWN LINKS & IMAGES -->
+<!-- https://www.markdownguide.org/basic-syntax/#reference-style-links -->
+[contributors-shield]: https://img.shields.io/github/contributors/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[contributors-url]: https://github.com/TeamSOBITS/ar_track_alvar/graphs/contributors
+[forks-shield]: https://img.shields.io/github/forks/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[forks-url]: https://github.com/TeamSOBITS/ar_track_alvar/network/members
+[stars-shield]: https://img.shields.io/github/stars/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[stars-url]: https://github.com/TeamSOBITS/ar_track_alvar/stargazers
+[issues-shield]: https://img.shields.io/github/issues/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[issues-url]: https://github.com/TeamSOBITS/ar_track_alvar/issues
+[license-shield]: https://img.shields.io/github/license/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+<!-- [license-url]: https://github.com/TeamSOBITS/ar_track_alvar/blob/master/LICENSE.txt -->

README.md

@@ -0,0 +1,98 @@
+# **ar_to_distance**
+<a name="readme-top"></a>
+
+[JP](README.md) | [EN](README_en.md)
+
+[![Contributors][contributors-shield]][contributors-url]
+[![Forks][forks-shield]][forks-url]
+[![Stargazers][stars-shield]][stars-url]
+[![Issues][issues-shield]][issues-url]
+<!-- [![MIT License][license-shield]][license-url] -->
+
+<!-- 目次 -->
+<details>
+  <summary>目次</summary>
+  <ol>
+    <li>
+      <a href="#概要">概要</a>
+    </li>
+    <li>
+      <a href="#環境構築">環境構築</a>
+      <ul>
+        <li><a href="#環境条件">環境条件</a></li>
+        <li><a href="#インストール方法">インストール方法</a></li>
+      </ul>
+    </li>
+    <li><a href="#実行・操作方法">実行・操作方法</a></li>
+    <li><a href="#マイルストーン">マイルストーン</a></li>
+    <li><a href="#変更履歴">変更履歴</a></li>
+    <!-- <li><a href="#contributing">Contributing</a></li> -->
+    <!-- <li><a href="#license">License</a></li> -->
+    <li><a href="#参考文献">参考文献</a></li>
+  </ol>
+</details>
+
+<!-- レポジトリの概要 -->
+## 概要
+- ARコードを読み取り距離を算出するパッケージ
+- ARコードからcamera基準のTFを送信します．
+- ※/ar_track_alvar/makers　にあるARマーカーを使用して検出してください．
+<p align="right">(<a href="#readme-top">上に戻る</a>)</p>
+
+<!-- セットアップ -->
+## セットアップ
+ [ROS wiki](http://wiki.ros.org/ar_track_alvar) を参照してください.
+
+### 環境条件
+以下に正常動作環境を示します．
+| System  | Version |
+| ------------- | ------------- |
+| Ubuntu    | 20.04 (Focal Fossa) |
+| ROS       | Noetic Ninjemys |
+
+<!-- 実行・操作方法 -->
+## 実行・操作方法
+<!-- デモの実行方法やスクリーンショットがあるとわかりやすくなるでしょう -->
+- Azure Kinect Launcher(Azure Kinectを使用して検出します)
+```
+$ roslaunch ar_track_alvar azure_kinect.launch
+```
+※その他のカメラを使用した場合は、トピック名とframe名を変更してください．
+- [Topに戻る](#ar_to_distance)
+
+<!-- マイルストーン -->
+## マイルストーン
+- [ ] OSS
+    - [ ] ドキュメンテーションの充実
+    - [ ] コーディングスタイルの統一
+
+現時点のバッグや新規機能の依頼を確認するために[Issueページ](https://github.com/ros-perception/ar_track_alvar/issues) をご覧ください．
+
+<p align="right">(<a href="#readme-top">上に</a>)</p>
+
+<!-- 変更履歴 -->
+## 変更履歴
+  変更履歴はrst.fileを参照してください
+<!-- 参考文献 -->
+## 参考文献
+
+* []()
+* []()
+* []()
+
+<p align="right">(<a href="#readme-top">上に戻る</a>)</p>
+
+
+
+<!-- MARKDOWN LINKS & IMAGES -->
+<!-- https://www.markdownguide.org/basic-syntax/#reference-style-links -->
+[contributors-shield]: https://img.shields.io/github/contributors/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[contributors-url]: https://github.com/TeamSOBITS/ar_track_alvar/graphs/contributors
+[forks-shield]: https://img.shields.io/github/forks/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[forks-url]: https://github.com/TeamSOBITS/ar_track_alvar/network/members
+[stars-shield]: https://img.shields.io/github/stars/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[stars-url]: https://github.com/TeamSOBITS/ar_track_alvar/stargazers
+[issues-shield]: https://img.shields.io/github/issues/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[issues-url]: https://github.com/TeamSOBITS/ar_track_alvar/issues
+[license-shield]: https://img.shields.io/github/license/TeamSOBITS/ar_track_alvar.svg?style=for-the-badge
+[license-url]: https://github.com/TeamSOBITS/ar_track_alvar/blob/master/LICENSE.txt

ar_track_alvar/launch/azure_kinect.launch

@@ -0,0 +1,15 @@
+<launch>
+	<arg name="marker_size" default="5.4" />
+	<arg name="max_new_marker_error" default="0.08" />
+	<arg name="max_track_error" default="0.2" />
+	<arg name="cam_image_topic" default="/rgb/image_raw" />
+	<arg name="cam_info_topic" default="/rgb/camera_info" />
+	<arg name="output_frame" default="/azure_camera_base" />
+
+	<!-- <arg name="cam_image_topic" default="/usb_cam/image_raw" />
+	<arg name="cam_info_topic" default="/usb_cam/camera_info" />
+	<arg name="output_frame" default="/usb_cam" /> -->
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" 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)" />
+</launch>

ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp

@@ -238,6 +238,13 @@ void configCallback(ar_track_alvar::ParamsConfig& config, uint32_t level)
 
   enableSwitched = enabled != config.enabled;
 
+  if (config.marker_size != marker_size)
+  {
+    marker_detector.SetMarkerSize(config.marker_size, marker_resolution, marker_margin);
+    marker_detector.TrackMarkersReset();
+    marker_detector.markers->clear();
+  }
+
   enabled = config.enabled;
   max_frequency = config.max_frequency;
   marker_size = config.marker_size;

ar_track_alvar/src/tf_broadcaster.cpp

@@ -0,0 +1,94 @@
+#include <iostream>
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int8.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_broadcaster.h>
+
+class BroadcasterNode{
+    private:
+        //initilize ros
+        ros::NodeHandle nh;
+        ros::Subscriber sub_visp_status;
+        ros::Subscriber sub_visp_name;
+        ros::Subscriber sub_visp_distance;
+
+        //TF
+        tf::TransformBroadcaster br;
+        std::string camera_frame_name;  
+
+        //QR parameters
+        int status_qr;
+        std::string object_name;
+        geometry_msgs::PoseStamped qr_pose;
+
+
+    public:
+        bool exec_flag;
+        BroadcasterNode();
+        void get_statusQR(std_msgs::Int8);
+        void get_nameQR(std_msgs::String);
+        void get_distanceQR(geometry_msgs::PoseStamped);
+        void tf_broadcaster();
+};
+
+BroadcasterNode::BroadcasterNode(){
+    this->sub_visp_status = nh.subscribe("/visp_auto_tracker/status",1,&BroadcasterNode::get_statusQR,this);
+    this->sub_visp_name = nh.subscribe("/visp_auto_tracker/code_message",1,&BroadcasterNode::get_nameQR,this);
+    this->sub_visp_distance = nh.subscribe("/visp_auto_tracker/object_position",1,&BroadcasterNode::get_distanceQR,this);
+    ROS_INFO("tf_broadcaster initialize ok");
+    if(exec_flag = true){
+        //tf_broadcaster();
+    }
+}
+
+void BroadcasterNode::get_statusQR(std_msgs::Int8 status){
+    this->status_qr = status.data;
+    //ROS_INFO("QR status: %s",status_qr);
+    //std::cout << status_qr << std::endl;
+    if(status_qr == 1) this->exec_flag = false;
+}
+
+void BroadcasterNode::get_nameQR(std_msgs::String qr_message){
+    this->object_name = qr_message.data;
+    //ROS_INFO("QR message: %s",status_qr);
+    //std::cout << object_name << std::endl;
+}
+
+void BroadcasterNode::get_distanceQR(geometry_msgs::PoseStamped qr_distance){
+    this->qr_pose = qr_distance;
+    this->exec_flag = true;
+    //ROS_INFO("QR pose: %ld",qr_pose);
+    //std::cout << qr_pose << std::endl;
+}
+
+void BroadcasterNode::tf_broadcaster(){
+    tf::Transform transform;
+    //set position
+    float obj_x = qr_pose.pose.position.x;
+    float obj_y = qr_pose.pose.position.y;
+    float obj_z = qr_pose.pose.position.z;
+    transform.setOrigin(tf::Vector3(obj_x,obj_y,obj_z));
+
+    //set rotation
+    float rot_x = qr_pose.pose.orientation.x;
+    float rot_y = qr_pose.pose.orientation.y;
+    float rot_z = qr_pose.pose.orientation.z;
+    float rot_w = qr_pose.pose.orientation.w;
+    tf::Quaternion q;
+    q.setValue(rot_x,rot_y,rot_z,rot_w);
+    transform.setRotation(q);
+
+    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"head_rgb_camera_link",object_name));
+}
+
+int main(int argc, char *argv[]){
+    ros::init(argc,argv,"tf_broadcaster_node");
+    BroadcasterNode bcn;
+    ros::Rate loop_rate(30);
+    while(ros::ok()){  
+        bcn.tf_broadcaster();
+        loop_rate.sleep();
+        ros::spinOnce();
+    }
+}

ar_track_alvar/test/test_ar.py

@@ -85,7 +85,7 @@ def test_markers(self):
             while not rospy.is_shutdown():
                 try:
                     target_frame = '/ar_marker_{}'.format(i)
-                    (trans, rot) = self.tflistener.lookupTransform('/camera', target_frame, rospy.Time(0))
+                    (trans, rot) = self.tflistener.lookupTransform('/camera_link', target_frame, rospy.Time(0))
                     break
                 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                     rospy.logerr(str(e) + ' target_frame={}'.format(target_frame))