diff --git a/.gitignore b/.gitignore index 070bb21..6d7d10c 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,10 @@ *bin *~ src/ar_track_alvar -*.user \ No newline at end of file +*.user +*.swp +GPATH +GRTAGS +GTAGS +GSYMS + diff --git a/ar_track_alvar/include/ar_track_alvar/Marker.h b/ar_track_alvar/include/ar_track_alvar/Marker.h index 0313833..880cef2 100644 --- a/ar_track_alvar/include/ar_track_alvar/Marker.h +++ b/ar_track_alvar/include/ar_track_alvar/Marker.h @@ -100,6 +100,8 @@ namespace alvar { void SetMarkerSize(double _edge_length = 0, int _res = 0, double _margin = 0); /** \brief Get edge length (to support different size markers */ double GetMarkerEdgeLength() const { return edge_length; } + /** \brief Method for Get the marker resolution */ + int GetMarkerRes(void); /** \brief Destructor */ ~Marker(); /** \brief Default constructor diff --git a/ar_track_alvar/launch/pr2_bundle.launch b/ar_track_alvar/launch/pr2_bundle.launch index 69d49da..e02ac2e 100755 --- a/ar_track_alvar/launch/pr2_bundle.launch +++ b/ar_track_alvar/launch/pr2_bundle.launch @@ -1,4 +1,5 @@ + @@ -10,5 +11,5 @@ - + diff --git a/ar_track_alvar/launch/pr2_bundle_no_kinect.launch b/ar_track_alvar/launch/pr2_bundle_no_kinect.launch index c7892dd..f2b4ac4 100755 --- a/ar_track_alvar/launch/pr2_bundle_no_kinect.launch +++ b/ar_track_alvar/launch/pr2_bundle_no_kinect.launch @@ -1,4 +1,5 @@ + @@ -9,5 +10,5 @@ - + diff --git a/ar_track_alvar/launch/pr2_indiv.launch b/ar_track_alvar/launch/pr2_indiv.launch index 27e4689..768ec21 100755 --- a/ar_track_alvar/launch/pr2_indiv.launch +++ b/ar_track_alvar/launch/pr2_indiv.launch @@ -1,4 +1,5 @@ + @@ -12,6 +13,7 @@ + diff --git a/ar_track_alvar/launch/pr2_indiv_no_kinect.launch b/ar_track_alvar/launch/pr2_indiv_no_kinect.launch index 4428714..867ead7 100755 --- a/ar_track_alvar/launch/pr2_indiv_no_kinect.launch +++ b/ar_track_alvar/launch/pr2_indiv_no_kinect.launch @@ -1,4 +1,5 @@ + @@ -11,6 +12,7 @@ + diff --git a/ar_track_alvar/nodes/FindMarkerBundles.cpp b/ar_track_alvar/nodes/FindMarkerBundles.cpp index e7f83e0..23c1f35 100644 --- a/ar_track_alvar/nodes/FindMarkerBundles.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundles.cpp @@ -112,6 +112,8 @@ std::string cam_info_topic; std::string output_frame; int n_bundles = 0; +bool inverse_tf = false; + //Debugging utility function void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad) { @@ -527,8 +529,13 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); - tf_broadcaster->sendTransform(camToMarker); + if (inverse_tf) { + tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame); + tf_broadcaster->sendTransform(markerToCam); + } else { + tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); + tf_broadcaster->sendTransform(camToMarker); + } //Create the rviz visualization message tf::poseTFToMsg (markerPose, rvizMarker->pose); @@ -771,10 +778,10 @@ int main(int argc, char *argv[]) ros::init (argc, argv, "marker_detect"); ros::NodeHandle n; - if(argc < 9){ + if(argc < 10){ std::cout << std::endl; cout << "Not enough arguments provided." << endl; - cout << "Usage: ./findMarkerBundles " << endl; + cout << "Usage: ./findMarkerBundles " << endl; std::cout << std::endl; return 0; } @@ -787,7 +794,12 @@ int main(int argc, char *argv[]) cam_info_topic = argv[5]; output_frame = argv[6]; med_filt_size = atoi(argv[7]); - int n_args_before_list = 8; + std::stringstream tmpss(argv[8]); + if(!(tmpss >> std::boolalpha >> inverse_tf)) { + // parse failed + inverse_tf = false; + } + int n_args_before_list = 9; n_bundles = argc - n_args_before_list; marker_detector.SetMarkerSize(marker_size); diff --git a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp index 013588b..6a96846 100644 --- a/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp +++ b/ar_track_alvar/nodes/FindMarkerBundlesNoKinect.cpp @@ -82,6 +82,8 @@ void GetMultiMarkerPoses(IplImage *image); void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg); void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker); +bool inverse_tf = false; + // Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position void GetMultiMarkerPoses(IplImage *image) { @@ -128,8 +130,14 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_ out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); - tf_broadcaster->sendTransform(camToMarker); + if (inverse_tf) { + tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame); + tf_broadcaster->sendTransform(markerToCam); + } else { + tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); + tf_broadcaster->sendTransform(camToMarker); + } + } //Create the rviz visualization message @@ -281,10 +289,10 @@ int main(int argc, char *argv[]) ros::init (argc, argv, "marker_detect"); ros::NodeHandle n; - if(argc < 8){ + if(argc < 9){ std::cout << std::endl; cout << "Not enough arguments provided." << endl; - cout << "Usage: ./findMarkerBundles " << endl; + cout << "Usage: ./findMarkerBundles " << endl; std::cout << std::endl; return 0; } @@ -296,7 +304,12 @@ int main(int argc, char *argv[]) cam_image_topic = argv[4]; cam_info_topic = argv[5]; output_frame = argv[6]; - int n_args_before_list = 7; + std::stringstream tmpss(argv[7]); + if(!(tmpss >> std::boolalpha >> inverse_tf)) { + // parse failed + inverse_tf = false; + } + int n_args_before_list = 8; n_bundles = argc - n_args_before_list; marker_detector.SetMarkerSize(marker_size); diff --git a/ar_track_alvar/nodes/IndividualMarkers.cpp b/ar_track_alvar/nodes/IndividualMarkers.cpp index a44a23f..8ec769a 100644 --- a/ar_track_alvar/nodes/IndividualMarkers.cpp +++ b/ar_track_alvar/nodes/IndividualMarkers.cpp @@ -86,6 +86,8 @@ std::string output_frame; int marker_resolution = 5; // default marker resolution int marker_margin = 2; // default marker margin +bool inverse_tf = false; + //Debugging utility function void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad) @@ -374,6 +376,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) { //Get the pose relative to the camera int id = (*(marker_detector.markers))[i].GetId(); + std::string content_str = (*(marker_detector.markers))[i].data.str; Pose p = (*(marker_detector.markers))[i].pose; double px = p.translation[0]/100.0; @@ -397,8 +400,13 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); - tf_broadcaster->sendTransform(camToMarker); + if (inverse_tf) { + tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame); + tf_broadcaster->sendTransform(markerToCam); + } else { + tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); + tf_broadcaster->sendTransform(camToMarker); + } //Create the rviz visualization messages tf::poseTFToMsg (markerPose, rvizMarker_.pose); @@ -463,6 +471,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg) ar_pose_marker.header.frame_id = output_frame; ar_pose_marker.header.stamp = image_msg->header.stamp; ar_pose_marker.id = id; + ar_pose_marker.str = content_str; arPoseMarkers_.markers.push_back (ar_pose_marker); } arPoseMarkers_.header.stamp = image_msg->header.stamp; @@ -500,7 +509,7 @@ int main(int argc, char *argv[]) std::cout << std::endl; cout << "Not enough arguments provided." << endl; cout << "Usage: ./individualMarkers " - << " [ ]"; + << " [ ]"; std::cout << std::endl; return 0; } @@ -522,6 +531,16 @@ int main(int argc, char *argv[]) if (argc > 9) marker_margin = atoi(argv[9]); + if (argc > 10) { + std::stringstream tmpss(argv[10]); + if(!(tmpss >> std::boolalpha >> inverse_tf)) { + // parse failed + inverse_tf = false; + } + } else { + inverse_tf = false; + } + } else { // Get params from ros param server. pn.param("marker_size", marker_size, 10.0); @@ -532,6 +551,7 @@ int main(int argc, char *argv[]) pn.param("marker_resolution", marker_resolution, 5); pn.param("marker_margin", marker_margin, 2); pn.param("output_frame_from_msg", output_frame_from_msg, false); + pn.param("inverse_tf", inverse_tf, false); if (!output_frame_from_msg && !pn.getParam("output_frame", output_frame)) { ROS_ERROR("Param 'output_frame' has to be set if the output frame is not " diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp index d5a439e..31c60cc 100644 --- a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp +++ b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp @@ -75,6 +75,8 @@ std::string output_frame; int marker_resolution = 5; // default marker resolution int marker_margin = 2; // default marker margin +bool inverse_tf = false; + void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg); @@ -109,6 +111,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) { //Get the pose relative to the camera int id = (*(marker_detector.markers))[i].GetId(); + std::string content_str = (*(marker_detector.markers))[i].data.str; Pose p = (*(marker_detector.markers))[i].pose; double px = p.translation[0]/100.0; double py = p.translation[1]/100.0; @@ -139,8 +142,14 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) out << id; std::string id_string = out.str(); markerFrame += id_string; - tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); - tf_broadcaster->sendTransform(camToMarker); + if (inverse_tf) { + tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame.c_str(), output_frame); + tf_broadcaster->sendTransform(markerToCam); + } else { + tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str()); + tf_broadcaster->sendTransform(camToMarker); + } + //Create the rviz visualization messages tf::poseTFToMsg (markerPose, rvizMarker_.pose); @@ -205,6 +214,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) ar_pose_marker.header.frame_id = output_frame; ar_pose_marker.header.stamp = image_msg->header.stamp; ar_pose_marker.id = id; + ar_pose_marker.str = content_str; arPoseMarkers_.markers.push_back (ar_pose_marker); } arMarkerPub_.publish (arPoseMarkers_); @@ -247,7 +257,7 @@ int main(int argc, char *argv[]) std::cout << std::endl; cout << "Not enough arguments provided." << endl; cout << "Usage: ./individualMarkersNoKinect " - << " [ ]"; + << " [ ]"; std::cout << std::endl; return 0; } @@ -268,6 +278,15 @@ int main(int argc, char *argv[]) marker_resolution = atoi(argv[8]); if (argc > 9) marker_margin = atoi(argv[9]); + if (argc > 10) { + std::stringstream tmpss(argv[10]); + if(!(tmpss >> std::boolalpha >> inverse_tf)) { + // parse failed + inverse_tf = false; + } + } else { + inverse_tf = false; + } } else { // Get params from ros param server. @@ -278,6 +297,7 @@ int main(int argc, char *argv[]) pn.setParam("max_frequency", max_frequency); // in case it was not set. pn.param("marker_resolution", marker_resolution, 5); pn.param("marker_margin", marker_margin, 2); + pn.param("inverse_tf", inverse_tf, false); if (!pn.getParam("output_frame", output_frame)) { ROS_ERROR("Param 'output_frame' has to be set."); exit(EXIT_FAILURE); diff --git a/ar_track_alvar/src/Marker.cpp b/ar_track_alvar/src/Marker.cpp index fdaf503..6d07804 100644 --- a/ar_track_alvar/src/Marker.cpp +++ b/ar_track_alvar/src/Marker.cpp @@ -512,6 +512,10 @@ void Marker::SetMarkerSize(double _edge_length, int _res, double _margin) { marker_content = cvCreateMat(res, res, CV_8U); cvSet(marker_content, cvScalar(255)); } +int Marker::GetMarkerRes(void){ + return res; +} + Marker::~Marker() { if (marker_content) cvReleaseMat(&marker_content); } @@ -838,7 +842,7 @@ void MarkerData::Read6bitStr(BitsetExt *bs, char *s, size_t s_max_len) { if (bitpos < 0) { if (c == 000) s[len] = ':'; else if ((c >= 001) && (c <= 032)) s[len] = 'a' + (char)c - 1; - else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 1; + else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 033; else if (c == 045) s[len] = '+'; else if (c == 046) s[len] = '-'; else if (c == 047) s[len] = '*'; diff --git a/ar_track_alvar/src/SampleMarkerCreator.cpp b/ar_track_alvar/src/SampleMarkerCreator.cpp index c0a8f04..97bd46b 100644 --- a/ar_track_alvar/src/SampleMarkerCreator.cpp +++ b/ar_track_alvar/src/SampleMarkerCreator.cpp @@ -1,5 +1,6 @@ #include "ar_track_alvar/MultiMarker.h" #include "highgui.h" +#include using namespace std; using namespace alvar; @@ -75,8 +76,9 @@ struct State { maxx = new_maxx; maxy = new_maxy; } if (marker_data_content_type == MarkerData::MARKER_CONTENT_TYPE_NUMBER) { - int idi = atoi(id); + long idi = atol(id); md.SetContent(marker_data_content_type, idi, 0); + filename<<"_res"< unusable_char = { + { '#', true }, + { '<', true }, + { '$', true }, + { '+', true }, + { '%', true }, + { '>', true }, + { '!', true }, + { '`', true }, + { '&', true }, + { '*', true }, + { '"', true }, + { '|', true }, + { '{', true }, + { '?', true }, + { '=', true }, + { '}', true }, + { '/', true }, + { ':', true }, + { '\\', true }, + { '\'', true }, + { '@', true } + }; while(*p) { - if (!isalnum(*p)) filename<<"_"; + //if (!isalnum(*p)) filename<<"-"; + if (!isalnum(*p) && unusable_char.find((char)*p) != unusable_char.end()) filename<<"-"; else filename<<(char)tolower(*p); p++; counter++; - if (counter > 8) break; + //if (counter > 8) break; + if (counter > 200) break; } } md.ScaleMarkerToImage(img); @@ -106,8 +134,9 @@ struct State { img = cvCreateImage(cvSize(side_len, side_len), IPL_DEPTH_8U, 1); filename.str(""); filename<<"MarkerArtoolkit"; - md.SetContent(atoi(id)); - filename<<"_"< 0) marker_id=atoi(s.c_str()); - if (marker_id < 0) break; + std::getline(std::cin, s); if (s.length() > 0) marker_id=atol(s.c_str()); + if (marker_id < 0 || marker_id > INT_MAX ) break; std::cout<<" x position (in current units) ["< 0) posx=atof(s.c_str()); std::cout<<" y position (in current units) ["< 0) column_major=atoi(s.c_str()); std::cout<<"First marker ID : "; std::flush(std::cout); - std::getline(std::cin, s); if (s.length() > 0) first_id=atoi(s.c_str()); + std::getline(std::cin, s); if (s.length() > 0) first_id=atol(s.c_str()); if(!column_major) { for(int row = 0; row