Artags config

rover/src/owr_positioning/CMakeLists.txt

@@ -123,6 +123,7 @@ add_executable(gps_tf src/GPSTransform.cpp)
 add_executable(mag src/MagnetConverter.cpp)
 add_executable(optical_localization src/optical_localization.cpp)
 add_executable(image_cropping src/ImageCropping.cpp)
+add_executable(create_artag_config src/create_artag_config.cpp)
 target_link_libraries(optical_localization ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
 set_target_properties(optical_localization PROPERTIES COMPILE_FLAGS "-O3")
 target_link_libraries(image_cropping ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
@@ -156,6 +157,10 @@ target_link_libraries(mag
         ${catkin_LIBRARIES}
 	${OpenCV_LIBRARIES}
         )
+target_link_libraries(create_artag_config
+   ${catkin_LIBRARIES}
+   ${OpenCV_LIBRARIES}
+)
 #   src/${PROJECT_NAME}/owr_positioning.cpp
 # )
 

rover/src/owr_positioning/launch/artags.launch

@@ -1,8 +1,5 @@
 <launch>
 
-  <!-- Create a static transform for each ar tag -->
-    <node pkg="tf" type="static_transform_publisher" name="marker0" args="2 1 0 0 0 0 world /marker0 100">
-    </node>
     <node pkg="tf" type="static_transform_publisher" name="base_point" args="0 0 0 0 0 0 world /base_point 100">
     </node>
 

rover/src/owr_positioning/launch/artags_location.launch

@@ -0,0 +1,5 @@
+<launch>
+<node pkg="tf" type="static_transform_publisher" name="marker6" args="5 0 0 0.5 0 0 world /marker6 100">
+<node pkg="tf" type="static_transform_publisher" name="marker7" args="10 5 0 0 0.5 0 world /marker7 100">
+<node pkg="tf" type="static_transform_publisher" name="marker3" args="10 5 5 0 0 0.5 world /marker3 100">
+</launch>

rover/src/owr_positioning/launch/create_artag_config.launch

@@ -0,0 +1,71 @@
+<launch>
+
+    <node pkg="tf" type="static_transform_publisher" name="base_point" args="0 0 0 0 0 0 world /base_point 100">
+    </node>
+
+  <!-- Setup the video stream from the camera -->
+   <arg name="camera_name" default="camera" />
+   <!-- video_stream_provider can be a number as a video device or a url of a video stream -->
+   <arg name="video_stream_provider" default="/dev/video0" />
+   <!-- frames per second to query the camera for -->
+   <arg name="fps" default="1" />
+   <!-- frame_id for the camera -->
+   <arg name="frame_id" default="camera_link" />
+   <!-- By default, calibrations are stored to file://${ROS_HOME}/camera_info/${NAME}.yaml
+   To use your own fill this arg with the corresponding url, e.g.:
+   "file:///$(find your_camera_package)/config/your_camera.yaml" -->
+    <arg name="camera_info_url" default="file:///home/ros/Desktop/webcamCalibration.yaml" />
+   <!-- flip the image horizontally (mirror it) -->
+   <arg name="flip_horizontal" default="false" />
+   <!-- flip the image vertically -->
+   <arg name="flip_vertical" default="false" />
+    <!-- force width and height, 0 means no forcing -->
+    <arg name="width" default="0"/>
+    <arg name="height" default="0"/>
+    <arg name="buffer_queue_size" default="1"/>
+   <!-- if show a image_view window subscribed to the generated stream -->
+ <arg name="visualize" default="true"/>
+
+
+    <!-- images will be published at /camera_name/image with the image transports plugins (e.g.: compressed) installed -->
+    <group ns="$(arg camera_name)">
+     <node pkg="video_stream_opencv" type="video_stream" name="$(arg camera_name)_stream" output="screen"> 
+      <remap from="camera" to="image_raw" />
+      <param name="camera_name" type="string" value="$(arg camera_name)" />
+         <param name="video_stream_provider" type="string" value="$(arg video_stream_provider)" />
+         <param name="fps" type="int" value="$(arg fps)" />
+         <param name="frame_id" type="string" value="$(arg frame_id)" />
+         <param name="camera_info_url" type="string" value="$(arg camera_info_url)" />
+         <param name="buffer_queue_size" type="int" value="$(arg buffer_queue_size)" />
+         <param name="flip_horizontal" type="bool" value="$(arg flip_horizontal)" />
+         <param name="flip_vertical" type="bool" value="$(arg flip_vertical)" />
+         <param name="width" type="int" value="$(arg width)" />
+         <param name="height" type="int" value="$(arg height)" />
+     </node>
+
+
+ </group>
+
+
+
+
+  <!-- Set up the ar_track_alvar package to scan for ar tags -->
+    <arg name="marker_size" default="5.6" />
+    <arg name="max_new_marker_error" default="0.10" />
+    <arg name="max_track_error" default="0.25" />
+    <arg name="cam_image_topic" default="/camera/image_raw" />
+    <arg name="cam_info_topic" default="/camera/camera_info" />
+    <arg name="output_frame" default="/camera_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>
+  <!-- node pkg="owr_positioning" type="create_artag_config" name="create_artag_config" output="screen"/ -->  
+</launch>

rover/src/owr_positioning/src/create_artag_config.cpp

@@ -0,0 +1,89 @@
+/*
+   Author : Abhishek Vijayan
+   Purpose : Create a launch file containing positions of AR Tags
+*/
+
+#include "artags.h"
+#include <string>
+#include <cstdlib>
+#include <fstream>
+
+#define DIR_LOCATION "/home/ros/owr_software/rover/src/owr_positioning/launch/"
+#define FILE_NAME "artags_location.launch"
+
+std::ofstream launch_file;
+bool add_artag = true; 
+
+int main(int argc, char ** argv) {
+    ROS_INFO("Launch file creator started");
+    ros::init(argc, argv, "arTag_localization"); 
+
+    std::string file_path = "";
+    file_path += DIR_LOCATION;
+    file_path += FILE_NAME;
+
+    ::launch_file.open(file_path.c_str());
+    if(not ::launch_file){
+       std::cout << "Could not create " << file_path <<"\n" ;
+       return EXIT_FAILURE;
+    }
+    else{
+    	::launch_file << "<launch>\n";
+    }
+    artag_localization arl;
+    arl.run();
+    if(::launch_file.is_open()){
+    	::launch_file << "</launch>\n";
+    	::launch_file.close(); 
+    	std::cout << "Successfully created " << file_path << "\n";
+    }
+    return EXIT_SUCCESS;
+}
+
+void artag_localization::run() {
+    while(ros::ok() && ::add_artag) {
+      ros::spinOnce();
+    }
+}
+
+artag_localization::artag_localization() : tfBuffer(), tfListener(tfBuffer)  {
+    //subscribe to the topic that provides the pose of observed ar tags
+    sub = nh.subscribe("/ar_pose_marker", 1, &artag_localization::callback, this);
+}
+
+void artag_localization::callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg) {
+    //find the amount of tags found
+
+    int size = msg->markers.size();
+    int id;
+    float x, y, z, x_r, y_r, z_r;
+    char response;
+
+    //for now we will only look at the first tag
+    if (size != 0){
+        //get the id of the first tag
+        id = msg->markers[0].id;
+
+   	if(::launch_file.is_open() && id != 255){
+		std::cout << "Detected ARTag : " << id << "\n";
+        	std::cout << "Enter x, y, z co-ordinates separated by space (or in different lines) : ";
+        	std::cin >> x >> y >> z;
+        	std::cout << "Enter rotation in x, y, z separated by space (or in different lines) : ";
+        	std::cin >> x_r >> y_r >> z_r;
+        	::launch_file << "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"marker"<< id 
+           		<<"\" args=\""
+           		<< x << " " << y << " " << z << " "
+           		<< x_r << " " << y_r << " " << z_r << " "
+           		<< "world /marker" 
+           		<< id << " 100\">\n";
+// <node pkg="tf" type="static_transform_publisher" name="marker0" args="2.0 1.0 0.0 0 0 0 world /marker0 100">
+      		std::cout << "Do you want to add details of another AR Tag ? (Y/N) : ";
+      		std::cin >> response;
+      		if(response == 'N'){
+              		::add_artag = false;
+      		}
+      	}
+
+    }
+
+}