├── CMakeLists.txt ├── README.md ├── launch └── vins_to_mavros.launch ├── package.xml └── src └── vins_to_mavros.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(vins_to_mavros) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | mavros_msgs 13 | nav_msgs 14 | roscpp 15 | std_msgs 16 | tf 17 | tf2_eigen 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # geometry_msgs# mavros_msgs# nav_msgs# std_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES vins_to_mavros 112 | # CATKIN_DEPENDS geometry_msgs mavros_msgs nav_msgs roscpp std_msgs tf tf2_eigen 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | add_executable(vins_to_mavros_node src/vins_to_mavros.cpp) 128 | target_link_libraries(vins_to_mavros_node 129 | ${catkin_LIBRARIES} 130 | ) 131 | 132 | ############# 133 | ## Install ## 134 | ############# 135 | 136 | # all install targets should use catkin DESTINATION variables 137 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 138 | 139 | ## Mark executable scripts (Python etc.) for installation 140 | ## in contrast to setup.py, you can choose the destination 141 | # catkin_install_python(PROGRAMS 142 | # scripts/my_python_script 143 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | # ) 145 | 146 | ## Mark executables for installation 147 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 148 | # install(TARGETS ${PROJECT_NAME}_node 149 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 150 | # ) 151 | 152 | ## Mark libraries for installation 153 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 154 | # install(TARGETS ${PROJECT_NAME} 155 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 156 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 157 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 158 | # ) 159 | 160 | ## Mark cpp header files for installation 161 | # install(DIRECTORY include/${PROJECT_NAME}/ 162 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 163 | # FILES_MATCHING PATTERN "*.h" 164 | # PATTERN ".svn" EXCLUDE 165 | # ) 166 | 167 | ## Mark other files for installation (e.g. launch and bag files, etc.) 168 | # install(FILES 169 | # # myfile1 170 | # # myfile2 171 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 172 | # ) 173 | 174 | ############# 175 | ## Testing ## 176 | ############# 177 | 178 | ## Add gtest based cpp test target and link libraries 179 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_vins_to_mavros.cpp) 180 | # if(TARGET ${PROJECT_NAME}-test) 181 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 182 | # endif() 183 | 184 | ## Add folders to be run by python nosetests 185 | # catkin_add_nosetests(test) 186 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # px4ctrl 2 | 3 | 一个简单的 vision_to_mavros 功能包 4 | 主要功能： 5 | （1）将 VINS-Fusion 的 body 坐标系在 world 坐标系下为位姿转化为 base_link 在 map 坐标系中的位姿； 6 | （2）将转化后的位姿信息以话题 /mavros/vision_pose/pose 发布。 7 | -------------------------------------------------------------------------------- /launch/vins_to_mavros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vins_to_mavros 4 | 0.0.0 5 | The vins_to_mavros package 6 | 7 | 8 | 9 | 10 | ocean 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | mavros_msgs 54 | nav_msgs 55 | roscpp 56 | std_msgs 57 | tf 58 | tf2_eigen 59 | geometry_msgs 60 | mavros_msgs 61 | nav_msgs 62 | roscpp 63 | std_msgs 64 | tf 65 | tf2_eigen 66 | geometry_msgs 67 | mavros_msgs 68 | nav_msgs 69 | roscpp 70 | std_msgs 71 | tf 72 | tf2_eigen 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /src/vins_to_mavros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | Eigen::Vector3d p_mav; 8 | Eigen::Quaterniond q_mav; 9 | 10 | 11 | void vins_callback(const nav_msgs::Odometry::ConstPtr &msg) 12 | { 13 | if(msg->header.frame_id == "world") 14 | { 15 | p_mav = Eigen::Vector3d(msg->pose.pose.position.y, -msg->pose.pose.position.x, msg->pose.pose.position.z); 16 | 17 | q_mav = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); 18 | Eigen::AngleAxisd roll(M_PI/2,Eigen::Vector3d::UnitX()); // 绕 x 轴旋转 pi / 2 19 | Eigen::AngleAxisd pitch(0,Eigen::Vector3d::UnitY()); 20 | Eigen::AngleAxisd yaw(0,Eigen::Vector3d::UnitZ()); 21 | 22 | Eigen::Quaterniond _q_mav = roll * pitch * yaw; 23 | q_mav = q_mav * _q_mav; 24 | } 25 | } 26 | 27 | 28 | int main(int argc, char **argv) 29 | { 30 | ros::init(argc, argv, "vins_to_mavros"); 31 | ros::NodeHandle nh("~"); 32 | 33 | ros::Subscriber slam_sub = nh.subscribe("odom", 100, vins_callback); 34 | 35 | ros::Publisher vision_pub = nh.advertise("vision_pose", 10); 36 | 37 | 38 | // the setpoint publishing rate MUST be faster than 2Hz 39 | ros::Rate rate(20.0); 40 | 41 | ros::Time last_request = ros::Time::now(); 42 | 43 | while(ros::ok()) { 44 | geometry_msgs::PoseStamped vision; 45 | 46 | vision.pose.position.x = p_mav[0]; 47 | vision.pose.position.y = p_mav[1]; 48 | vision.pose.position.z = p_mav[2]; 49 | 50 | vision.pose.orientation.x = q_mav.x(); 51 | vision.pose.orientation.y = q_mav.y(); 52 | vision.pose.orientation.z = q_mav.z(); 53 | vision.pose.orientation.w = q_mav.w(); 54 | 55 | vision.header.stamp = ros::Time::now(); 56 | vision_pub.publish(vision); 57 | 58 | ROS_INFO("\nposition:\n x: %.18f\n y: %.18f\n z: %.18f\norientation:\n x: %.18f\n y: %.18f\n z: %.18f\n w: %.18f", \ 59 | p_mav[0],p_mav[1],p_mav[2],q_mav.x(),q_mav.y(),q_mav.z(),q_mav.w()); 60 | 61 | ros::spinOnce(); 62 | rate.sleep(); 63 | } 64 | 65 | return 0; 66 | } 67 | 68 | 69 | --------------------------------------------------------------------------------