ar_sys: system_viewer.cpp Source File

Go to the documentation of this file.
00001 
00009 #include <opencv2/core/core.hpp>
00010 #include <aruco/aruco.h>
00011 
00012 #include <ros/ros.h>
00013 #include <tf/transform_broadcaster.h>
00014 #include <visualization_msgs/Marker.h>
00015 
00016 #define VISUAL_MARKER_SIZE      0.08
00017 
00018 enum position_t
00019 {
00020         FIXED,
00021         RELATIVE,
00022         FLOATING,
00023 
00024         POSITION_NR_ITEMS
00025 };
00026 
00027 struct board_t
00028 {
00029         int uid;
00030         std::string name;
00031         position_t type;
00032 
00033         std::string relativeName;
00034         tf::Transform transform;
00035 };
00036 
00037 struct relative_board_t
00038 {
00039         board_t board;
00040         std::map<std::string, tf::Transform> transforms_map;
00041 };
00042 
00043 struct camera_t
00044 {
00045         int uid;
00046         std::string name;
00047         position_t type;
00048 
00049         tf::Transform transform;
00050 };
00051 
00052 class ArSysViewer
00053 {
00054         private:
00055                 std::string map_path;
00056                 std::map<std::string, board_t> boards_map;
00057                 std::map<std::string, relative_board_t> relativeBoards_map;
00058                 std::map<std::string, camera_t> cameras_map;
00059                 double digital_filter_change_rate;
00060 
00061                 ros::Subscriber transform_sub;
00062                 ros::Publisher transform_pub; 
00063                 ros::Publisher rviz_marker_pub;
00064                 tf::TransformBroadcaster broadcaster;
00065                 ros::NodeHandle nh;
00066 
00067         public:
00068                 ArSysViewer()
00069                         : nh("~")
00070                 {
00071                         transform_sub = nh.subscribe ("/transform", 1, &ArSysViewer::transform_callback, this);
00072 
00073                         transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
00074                         rviz_marker_pub = nh.advertise <visualization_msgs::Marker> ("visualization_marker", 0);
00075 
00076                         nh.param<std::string>("map_path", map_path, "map.yml");
00077                         nh.param<double>("digital_filter_change_rate", digital_filter_change_rate, 0.5);
00078 
00079                         readFromFile(map_path.c_str());
00080                         ROS_INFO("ArSys node started with boards map: %s", map_path.c_str());
00081                 }
00082 
00083                 void readFromFile ( string sfile ) throw ( cv::Exception )
00084                 {
00085                         try
00086                         {
00087                                 cv::FileStorage fs ( sfile,cv::FileStorage::READ );
00088                                 readFromFile ( fs );
00089                         }
00090                         catch (std::exception &ex)
00091                         {
00092                                 throw   cv::Exception ( 81818,"ArSysViewer::readFromFile",ex.what()+string(" file=)")+sfile ,__FILE__,__LINE__ );
00093                         }
00094                 }
00095 
00096 
00097                 void readFromFile ( cv::FileStorage &fs ) throw ( cv::Exception )
00098                 {
00099                         //look for the ar_map_boards and ar_map_cameras
00100                         if (fs["ar_map_boards"].name() != "ar_map_boards" || fs["ar_map_cameras"].name() != "ar_map_cameras")
00101                                 throw cv::Exception ( 81818,"ArSysViewer::readFromFile","invalid file type" ,__FILE__,__LINE__ );
00102 
00103                         cv::FileNode FnBoards=fs["ar_map_boards"];
00104                         for (cv::FileNodeIterator it = FnBoards.begin(); it != FnBoards.end(); ++it)
00105                         {
00106                                 board_t board;
00107 
00108                                 board.uid = boards_map.size();
00109                                 board.name = (std::string)(*it)["name"];
00110                                 if ((std::string)(*it)["type"] == "fixed")
00111                                 {
00112                                         board.type = FIXED;
00113 
00114                                 }
00115                                 else if ((std::string)(*it)["type"] == "relative")
00116                                 {
00117                                         board.type = RELATIVE;
00118                                         board.relativeName = (std::string)(*it)["relativeName"];
00119                                 }
00120                                 else
00121                                 {
00122                                         if ((std::string)(*it)["type"] != "floating") ROS_WARN("Unknow board type: %s for board %s, assuming floating",
00123                                                 ((std::string)(*it)["type"]).c_str(),
00124                                                 board.name.c_str());
00125                                         board.type = FLOATING;
00126                                         board.transform = tf::Transform::getIdentity(); //Initial position
00127                                 }
00128                                 if (board.type == FIXED || board.type == RELATIVE)
00129                                 {
00130                                         vector<float> coordinates3d;
00131                                         cv::FileNode FnPose = (*it)["pose"];
00132                                         cv::FileNodeIterator itc = FnPose.begin();
00133 
00134                                         (*itc)>>coordinates3d;
00135                                         if (coordinates3d.size() != 3)
00136                                                 throw cv::Exception ( 81818,"ArSysViewer::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00137                                         tf::Vector3 ref_origin(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
00138 
00139                                         (*++itc)>>coordinates3d;
00140                                         if (coordinates3d.size() != 3)
00141                                                 throw cv::Exception ( 81818,"ArSysViewer::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00142                                         tf::Quaternion ref_rotation = tf::createQuaternionFromRPY(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
00143 
00144                                         board.transform = tf::Transform(ref_rotation, ref_origin);
00145 
00146                                         if (board.type == RELATIVE)
00147                                         {
00148                                                 if (relativeBoards_map.count(board.relativeName) == 0)
00149                                                 {
00150                                                         relative_board_t relativeBoard;
00151 
00152                                                         relativeBoard.board.uid = relativeBoards_map.size();
00153                                                         relativeBoard.board.name = board.relativeName;
00154                                                         relativeBoard.board.type = FLOATING;
00155                                                         relativeBoard.board.transform = tf::Transform::getIdentity(); //Initial position
00156 
00157                                                         relativeBoards_map.insert(std::make_pair(relativeBoard.board.name, relativeBoard));
00158                                                 }
00159                                                 relativeBoards_map[board.relativeName].transforms_map.insert(std::make_pair(board.name, board.transform));
00160                                                 board.transform = tf::Transform::getIdentity(); //Done with relative, Initial position
00161                                         }
00162                                 }
00163 
00164                                 boards_map.insert(std::make_pair(board.name, board));
00165                         }
00166 
00167                         cv::FileNode FnCameras=fs["ar_map_cameras"];
00168                         for (cv::FileNodeIterator it = FnCameras.begin(); it != FnCameras.end(); ++it)
00169                         {
00170                                 camera_t camera;
00171 
00172                                 camera.uid = cameras_map.size();
00173                                 camera.name = (std::string)(*it)["name"];
00174                                 if ((std::string)(*it)["type"] == "fixed")
00175                                 {
00176                                         camera.type = FIXED;
00177 
00178                                         vector<float> coordinates3d;
00179                                         cv::FileNode FnPose = (*it)["pose"];
00180                                         cv::FileNodeIterator itc = FnPose.begin();
00181 
00182                                         (*itc)>>coordinates3d;
00183                                         if (coordinates3d.size() != 3)
00184                                                 throw cv::Exception ( 81818,"ArSysViewer::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00185                                         tf::Vector3 ref_origin(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
00186 
00187                                         (*++itc)>>coordinates3d;
00188                                         if (coordinates3d.size() != 3)
00189                                                 throw cv::Exception ( 81818,"ArSysViewer::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00190                                         tf::Quaternion ref_rotation = tf::createQuaternionFromRPY(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
00191 
00192                                         camera.transform = tf::Transform(ref_rotation, ref_origin);
00193                                 }
00194                                 else
00195                                 {
00196                                         if ((std::string)(*it)["type"] != "floating") ROS_WARN("Unknow camera type: %s for camera %s, assuming floating",
00197                                                 ((std::string)(*it)["type"]).c_str(),
00198                                                 camera.name.c_str());
00199                                         camera.type = FLOATING;
00200                                         camera.transform = tf::Transform::getIdentity(); //Initial position
00201                                 }
00202 
00203                                 cameras_map.insert(std::make_pair(camera.name, camera));
00204                         }
00205                 }
00206 
00207                 tf::Transform& digital_filter(tf::Transform &dst, const tf::Transform &src)
00208                 {
00209                         //dst = src;
00210 
00211                         tf::Vector3 posOld = dst.getOrigin();
00212                         tf::Vector3 posNew = src.getOrigin();
00213                         tf::Vector3 pos
00214                         (
00215                                 (1 - digital_filter_change_rate) * posOld.x() + digital_filter_change_rate * posNew.x(),
00216                                 (1 - digital_filter_change_rate) * posOld.y() + digital_filter_change_rate * posNew.y(),
00217                                 (1 - digital_filter_change_rate) * posOld.z() + digital_filter_change_rate * posNew.z()
00218                         );
00219                         dst.setOrigin(pos);
00220 
00221                         tf::Quaternion ornOld = dst.getRotation();
00222                         tf::Quaternion ornNew = src.getRotation();
00223                         tf::Quaternion orn
00224                         (
00225                                 (1 - digital_filter_change_rate) * ornOld.x() + digital_filter_change_rate * ornNew.x(),
00226                                 (1 - digital_filter_change_rate) * ornOld.y() + digital_filter_change_rate * ornNew.y(),
00227                                 (1 - digital_filter_change_rate) * ornOld.z() + digital_filter_change_rate * ornNew.z(),
00228                                 (1 - digital_filter_change_rate) * ornOld.w() + digital_filter_change_rate * ornNew.w()
00229                         );
00230                         dst.setRotation(orn);
00231 
00232                         return dst;
00233                 }
00234 
00235                 void transform_callback (const geometry_msgs::TransformStamped& transformMsg)
00236                 {
00237                         if (cameras_map.count(transformMsg.header.frame_id) == 0)
00238                         {
00239                                 ROS_WARN("Map file does not contain an entry for the '%s' camera! Adding as floating", transformMsg.header.frame_id.c_str());
00240 
00241                                 camera_t camera;
00242                                 camera.uid = cameras_map.size();
00243                                 camera.name = transformMsg.header.frame_id;
00244                                 camera.type = FLOATING;
00245                                 camera.transform = tf::Transform::getIdentity(); //Initial position
00246                                 cameras_map.insert(std::make_pair(camera.name, camera));
00247                         }
00248                         if (boards_map.count(transformMsg.child_frame_id) == 0)
00249                         {
00250                                 ROS_WARN("Map file does not contain an entry for the '%s' board! Adding as floating", transformMsg.child_frame_id.c_str());
00251 
00252                                 board_t board;
00253                                 board.uid = boards_map.size();
00254                                 board.name = transformMsg.child_frame_id;
00255                                 board.type = FLOATING;
00256                                 board.transform = tf::Transform::getIdentity(); //Initial position
00257                                 boards_map.insert(std::make_pair(board.name, board));
00258                         }
00259 
00260                         tf::StampedTransform stampedTransform;
00261                         tf::transformStampedMsgToTF(transformMsg, stampedTransform);
00262                         if (cameras_map[transformMsg.header.frame_id].type == FLOATING && boards_map[transformMsg.child_frame_id].type == FIXED)
00263                         {
00264                                 digital_filter(cameras_map[transformMsg.header.frame_id].transform, boards_map[transformMsg.child_frame_id].transform * stampedTransform.inverse());
00265                         }
00266                         tf::StampedTransform camStampedTransform (cameras_map[transformMsg.header.frame_id].transform, transformMsg.header.stamp, "world", transformMsg.header.frame_id);
00267                         broadcaster.sendTransform(camStampedTransform);
00268 
00269                         stampedTransform.setData(boards_map[transformMsg.child_frame_id].type == FIXED ?
00270                                 boards_map[transformMsg.child_frame_id].transform :
00271                                 digital_filter(boards_map[transformMsg.child_frame_id].transform, cameras_map[transformMsg.header.frame_id].transform * stampedTransform));
00272                         stampedTransform.frame_id_ = camStampedTransform.frame_id_;
00273                         broadcaster.sendTransform(stampedTransform);
00274                         geometry_msgs::TransformStamped absTransformMsg;
00275                         tf::transformStampedTFToMsg(stampedTransform, absTransformMsg);
00276                         transform_pub.publish(absTransformMsg);
00277                         if (boards_map[transformMsg.child_frame_id].type == RELATIVE)
00278                         {
00279                                 std::string relativeName = boards_map[transformMsg.child_frame_id].relativeName;
00280 
00281                                 digital_filter(relativeBoards_map[relativeName].board.transform, boards_map[transformMsg.child_frame_id].transform * relativeBoards_map[relativeName].transforms_map[transformMsg.child_frame_id]);
00282                                 tf::StampedTransform relativeStampedTransform(relativeBoards_map[relativeName].board.transform, transformMsg.header.stamp, "world", relativeName);
00283                                 broadcaster.sendTransform(relativeStampedTransform);
00284                                 tf::transformStampedTFToMsg(relativeStampedTransform, absTransformMsg);
00285                                 transform_pub.publish(absTransformMsg);
00286                         }
00287 
00288                         //Generate visual marker
00289                         tf::Vector3 markerOrigin (0, 0, 0.1 * VISUAL_MARKER_SIZE);
00290                         tf::Transform m (tf::Quaternion::getIdentity(), markerOrigin);
00291                         tf::Transform markerPose = boards_map[transformMsg.child_frame_id].transform * m;
00292 
00293                         visualization_msgs::Marker visualMarker;
00294                         tf::poseTFToMsg (markerPose, visualMarker.pose);
00295 
00296                         visualMarker.header.frame_id = stampedTransform.frame_id_;
00297                         visualMarker.header.stamp = transformMsg.header.stamp;
00298                         visualMarker.id = boards_map[transformMsg.child_frame_id].uid;
00299 
00300                         visualMarker.scale.x = 1.0 * VISUAL_MARKER_SIZE;
00301                         visualMarker.scale.y = 1.0 * VISUAL_MARKER_SIZE;
00302                         visualMarker.scale.z = 0.1 * VISUAL_MARKER_SIZE;
00303                         visualMarker.ns = "basic_shapes";
00304                         visualMarker.type = visualization_msgs::Marker::CUBE;
00305                         visualMarker.action = visualization_msgs::Marker::ADD;
00306                         visualMarker.color.r = (float)visualMarker.id / (boards_map.size() - 1);
00307                         visualMarker.color.g = 1.0 - visualMarker.color.r;
00308                         visualMarker.color.b = (float)boards_map[transformMsg.child_frame_id].type / (POSITION_NR_ITEMS - 1);
00309                         visualMarker.color.a = 1.0;
00310                         visualMarker.lifetime = ros::Duration (1.0);
00311                         //Publish visual marker
00312                         rviz_marker_pub.publish(visualMarker);
00313                 }
00314 };
00315 
00316 
00317 int main(int argc,char **argv)
00318 {
00319         ros::init(argc, argv, "ar_system_viewer");
00320 
00321         ArSysViewer node;
00322 
00323         ros::spin();
00324 }