diff --git a/rover/src/owr_positioning/CMakeLists.txt b/rover/src/owr_positioning/CMakeLists.txt index 4a57d736..35e50d4b 100644 --- a/rover/src/owr_positioning/CMakeLists.txt +++ b/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 # ) diff --git a/rover/src/owr_positioning/launch/artags.launch b/rover/src/owr_positioning/launch/artags.launch index 88269e76..529ec735 100644 --- a/rover/src/owr_positioning/launch/artags.launch +++ b/rover/src/owr_positioning/launch/artags.launch @@ -1,8 +1,5 @@ - - - diff --git a/rover/src/owr_positioning/launch/artags_location.launch b/rover/src/owr_positioning/launch/artags_location.launch new file mode 100644 index 00000000..d44a1853 --- /dev/null +++ b/rover/src/owr_positioning/launch/artags_location.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/rover/src/owr_positioning/launch/create_artag_config.launch b/rover/src/owr_positioning/launch/create_artag_config.launch new file mode 100644 index 00000000..70249ca0 --- /dev/null +++ b/rover/src/owr_positioning/launch/create_artag_config.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rover/src/owr_positioning/src/create_artag_config.cpp b/rover/src/owr_positioning/src/create_artag_config.cpp new file mode 100644 index 00000000..2ccad3f1 --- /dev/null +++ b/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 +#include +#include + +#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 << "\n"; + } + artag_localization arl; + arl.run(); + if(::launch_file.is_open()){ + ::launch_file << "\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 << "\n"; +// + std::cout << "Do you want to add details of another AR Tag ? (Y/N) : "; + std::cin >> response; + if(response == 'N'){ + ::add_artag = false; + } + } + + } + +}