int main(int argc, char *argv[]) { std::string user_agent_string("Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Ubuntu Chromium/30.0.1599.114 Chrome/30.0.1599.114 Safari/537.36"); std::string yaml_file("../regexes.yaml"); if (argc>1 && std::string(argv[1])=="--help") { std::cout << "Usage: ua_parser_cli [user agent string] [regexes.yaml]" << std::endl; return 1; } else if (argc>1) user_agent_string = argv[1]; if (argc>2) yaml_file = argv[2]; ua_parser::Parser uap(yaml_file); ua_parser::UserAgent ua = uap.Parse(user_agent_string); if (ua.browser.family.empty()) return 2; std::cout << ua.browser.family << std::endl; std::cout << ua.os.os << std::endl; std::cout << ua.device << std::endl; }
int main( int argc, char** argv) { // initialize node ros::init(argc, argv, "marker_calibration"); ros::NodeHandle node_handle; ros::NodeHandle private_ns_node_handle("~"); unsigned int nr_of_samples=50; // subscribe to topic ros::Subscriber ar_pose_marker_subscriber = node_handle.subscribe("/ar_pose_marker", 10, ar_pose_marker_callback); // wait for ros::Rate sleep_rate(1.0); while( ros::ok() && nr_of_samples_received<nr_of_samples ){ ros::spinOnce(); sleep_rate.sleep(); printf("got %i samples\n",nr_of_samples_received); } if( nr_of_samples<=nr_of_samples_received ){ printf("received enough markers\n"); printf("marker_0 = %i, marker_1 = %i, marker_2 = %i\n", (int) marker0_pose.size(), (int) marker1_pose.size(), (int) marker2_pose.size()); ROS_INFO("average result"); print_statistic_attributes( marker0_pose ); print_statistic_attributes( marker1_pose ); print_statistic_attributes( marker2_pose ); geometry_msgs::Pose marker0_pose_avg; marker0_pose_avg.position = calc_position_average( marker0_pose ); marker0_pose_avg.orientation = calc_quaternion_average( marker0_pose ); geometry_msgs::Pose marker1_pose_avg; marker1_pose_avg.position = calc_position_average( marker1_pose ); marker1_pose_avg.orientation = calc_quaternion_average( marker1_pose ); geometry_msgs::Pose marker2_pose_avg; marker2_pose_avg.position = calc_position_average( marker2_pose ); marker2_pose_avg.orientation = calc_quaternion_average( marker2_pose ); if( marker0_frame_id.compare(marker1_frame_id)!=0 || marker1_frame_id.compare(marker2_frame_id)!=0 ){ ROS_ERROR("no common axis found"); return -1; } tf::Transform marker0_to_common_axis, marker1_to_common_axis, marker2_to_common_axis; tf::Transform marker0_to_map, marker1_to_map, marker2_to_map; tf::poseMsgToTF( marker0_pose_avg, marker0_to_common_axis); tf::poseMsgToTF( marker1_pose_avg, marker1_to_common_axis); tf::poseMsgToTF( marker2_pose_avg, marker2_to_common_axis); // calc transformation marker0_to_map marker0_to_map.getOrigin()[0] = (marker1_to_common_axis.inverse()*marker0_to_common_axis.getOrigin())[2]; marker0_to_map.getOrigin()[1] = 0.0; marker0_to_map.getOrigin()[2] = (marker2_to_common_axis.inverse()*marker0_to_common_axis.getOrigin())[2]; marker0_to_map.setBasis( tf::Matrix3x3(1, 0, 0, 0, 0, 1, 0,-1, 0)); // calc transformation marker1_to_map marker1_to_map.getOrigin()[0] = 0.0; marker1_to_map.getOrigin()[1] = (marker0_to_common_axis.inverse()*marker1_to_common_axis.getOrigin())[2]; marker1_to_map.getOrigin()[2] = (marker2_to_common_axis.inverse()*marker1_to_common_axis.getOrigin())[2]; marker1_to_map.setBasis( tf::Matrix3x3(0, 0, 1, 1, 0, 0, 0, 1, 0)); // calc transformation marker1_to_map marker2_to_map.getOrigin()[0] = (marker1_to_common_axis.inverse()*marker2_to_common_axis.getOrigin())[2]; marker2_to_map.getOrigin()[1] = (marker0_to_common_axis.inverse()*marker2_to_common_axis.getOrigin())[2]; marker2_to_map.getOrigin()[2] = 0.0; marker2_to_map.setBasis( tf::Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1)); tf::Transform map_to_common_axis0, map_to_common_axis1, map_to_common_axis2; map_to_common_axis0 = marker0_to_common_axis*marker0_to_map.inverse(); map_to_common_axis1 = marker1_to_common_axis*marker1_to_map.inverse(); map_to_common_axis2 = marker2_to_common_axis*marker2_to_map.inverse(); printf("marker 2 2 common axis = [%f,%f,%f]\n", marker2_to_common_axis.getOrigin()[0], marker2_to_common_axis.getOrigin()[1], marker2_to_common_axis.getOrigin()[2]); printf("marker 2 2 map = [%f,%f,%f]\n", marker2_to_map.getOrigin()[0], marker2_to_map.getOrigin()[1], marker2_to_map.getOrigin()[2]); printf("map 2 marker 2 = [%f,%f,%f]\n", marker2_to_map.inverse().getOrigin()[0], marker2_to_map.inverse().getOrigin()[1], marker2_to_map.inverse().getOrigin()[2]); printf("map 2 common axis 2 = [%f,%f,%f]\n", map_to_common_axis2.getOrigin()[0], map_to_common_axis2.getOrigin()[1], map_to_common_axis2.getOrigin()[2]); std::vector<tf::Transform> map_to_common_axis_vector; map_to_common_axis_vector.push_back( map_to_common_axis0); map_to_common_axis_vector.push_back( map_to_common_axis1); map_to_common_axis_vector.push_back( map_to_common_axis2); tf::Transform map_to_common_axis_avg = calc_pose_average( map_to_common_axis_vector); static tf::TransformBroadcaster br; printf("map 2 common axis = [%f,%f,%f]\n", map_to_common_axis_avg.getOrigin()[0], map_to_common_axis_avg.getOrigin()[1], map_to_common_axis_avg.getOrigin()[2]); printf("map 2 common axis avg = [%f,%f,%f]\n", map_to_common_axis_avg.getOrigin()[0], map_to_common_axis_avg.getOrigin()[1], map_to_common_axis_avg.getOrigin()[2]); tf::Transform marker0_to_map_avg = map_to_common_axis_avg.inverse()*marker0_to_common_axis; tf::Transform marker1_to_map_avg = map_to_common_axis_avg.inverse()*marker1_to_common_axis; tf::Transform marker2_to_map_avg = map_to_common_axis_avg.inverse()*marker2_to_common_axis; ROS_INFO("save parameters"); std::ofstream yaml_file( "/home/omnirob/catkin_ws/src/omnirob_robin/omnirob_robin_object_detection/data/marker_calibration.yaml" ); yaml_file << "marker_ids: [0, 1, 2]\n"; yaml_file << "marker0:\n"; // <!-- described in map coordinates --> yaml_file << " position:\n"; yaml_file << " x: " << marker0_to_map_avg.getOrigin()[0] <<"\n"; yaml_file << " y: " << marker0_to_map_avg.getOrigin()[1] <<"\n"; yaml_file << " z: " << marker0_to_map_avg.getOrigin()[2] <<"\n"; yaml_file << " orientation:\n"; yaml_file << " x: " << marker0_to_map_avg.getRotation()[0] <<"\n"; yaml_file << " y: " << marker0_to_map_avg.getRotation()[1] <<"\n"; yaml_file << " z: " << marker0_to_map_avg.getRotation()[2] <<"\n"; yaml_file << " w: " << marker0_to_map_avg.getRotation()[3] <<"\n"; yaml_file << "marker1:\n"; // <!-- described in map coordinates --> yaml_file << " position:\n"; yaml_file << " x: " << marker1_to_map_avg.getOrigin()[0] <<"\n"; yaml_file << " y: " << marker1_to_map_avg.getOrigin()[1] <<"\n"; yaml_file << " z: " << marker1_to_map_avg.getOrigin()[2] <<"\n"; yaml_file << " orientation:\n"; yaml_file << " x: " << marker1_to_map_avg.getRotation()[0] <<"\n"; yaml_file << " y: " << marker1_to_map_avg.getRotation()[1] <<"\n"; yaml_file << " z: " << marker1_to_map_avg.getRotation()[2] <<"\n"; yaml_file << " w: " << marker1_to_map_avg.getRotation()[3] <<"\n"; yaml_file << "marker2:\n"; // <!-- described in map coordinates --> yaml_file << " position:\n"; yaml_file << " x: " << marker2_to_map_avg.getOrigin()[0] <<"\n"; yaml_file << " y: " << marker2_to_map_avg.getOrigin()[1] <<"\n"; yaml_file << " z: " << marker2_to_map_avg.getOrigin()[2] <<"\n"; yaml_file << " orientation:\n"; yaml_file << " x: " << marker2_to_map_avg.getRotation()[0] <<"\n"; yaml_file << " y: " << marker2_to_map_avg.getRotation()[1] <<"\n"; yaml_file << " z: " << marker2_to_map_avg.getRotation()[2] <<"\n"; yaml_file << " w: " << marker2_to_map_avg.getRotation()[3] <<" \n"; yaml_file.close(); while( ros::ok() ){ br.sendTransform(tf::StampedTransform(marker0_to_common_axis, ros::Time::now(), marker0_frame_id, "marker0_avg")); br.sendTransform(tf::StampedTransform(marker1_to_common_axis, ros::Time::now(), marker1_frame_id, "marker1_avg")); br.sendTransform(tf::StampedTransform(marker2_to_common_axis, ros::Time::now(), marker2_frame_id, "marker2_avg")); br.sendTransform(tf::StampedTransform(map_to_common_axis0, ros::Time::now(), marker0_frame_id, "init_frame_0")); br.sendTransform(tf::StampedTransform(map_to_common_axis1, ros::Time::now(), marker1_frame_id, "init_frame_1")); br.sendTransform(tf::StampedTransform(map_to_common_axis2, ros::Time::now(), marker2_frame_id, "init_frame_2")); br.sendTransform(tf::StampedTransform(map_to_common_axis_avg, ros::Time::now(), marker2_frame_id, "init_frame_avg")); sleep_rate.sleep(); } } }