├── README.md ├── racecar ├── ssh.sh ├── map │ ├── test.pgm │ ├── mymap.pgm │ ├── test.yaml │ ├── test1.pgm │ ├── test1.yaml │ └── mymap.yaml ├── udev │ ├── README │ ├── car.rules │ ├── encoder.rules │ ├── laser.rules │ ├── racecar_init.sh │ └── imu.rules ├── save_map.sh ├── src │ ├── lib │ │ └── libracecar_controller.a │ ├── PID.h │ ├── car_controller.hpp │ ├── ESC_calibration.py │ ├── racecar_joy.py │ └── racecar_teleop.py ├── launch │ ├── teleop_joy.launch │ ├── rviz.launch │ ├── car.launch │ ├── includes │ │ ├── car_tf.launch.xml │ │ ├── rplidar.launch.xml │ │ ├── ls01g.launch.xml │ │ ├── rf2o.launch.xml │ │ ├── default_mapping.launch.xml │ │ └── amcl.launch.xml │ ├── ls01g_lidar.launch │ ├── ls01b_lidar.launch │ ├── Run_car.launch │ ├── amcl_nav_no_control.launch │ ├── Run_gmapping.launch │ ├── amcl_nav.launch │ └── amcl_nav copy.launch ├── cfg │ └── imu.cfg ├── param │ ├── move_base_params.yaml │ ├── base_global_planner_params.yaml │ ├── dwa_local_planner_params.yaml │ ├── local_costmap_params.yaml │ └── global_costmap_params.yaml ├── racecar_rviz.sh ├── README.md ├── package.xml └── rviz │ └── test_laser.rviz ├── imu_tool ├── rviz_imu_plugin │ ├── rosdoc.yaml │ ├── rviz_imu_plugin.png │ ├── plugin_description.xml │ ├── package.xml │ ├── CMakeLists.txt │ ├── CHANGELOG.rst │ └── src │ │ ├── imu_axes_visual.h │ │ ├── imu_acc_visual.h │ │ ├── imu_orientation_visual.h │ │ ├── imu_axes_visual.cpp │ │ ├── imu_acc_visual.cpp │ │ └── imu_display.h ├── imu_filter_madgwick │ ├── sample │ │ ├── ardrone_imu.bag │ │ ├── sparkfun_razor.bag │ │ └── phidgets_imu_upside_down.bag │ ├── include │ │ └── imu_filter_madgwick │ │ │ ├── world_frame.h │ │ │ ├── imu_filter_nodelet.h │ │ │ ├── stateless_orientation.h │ │ │ ├── imu_filter.h │ │ │ └── imu_filter_ros.h │ ├── imu_filter_nodelet.xml │ ├── cfg │ │ └── ImuFilterMadgwick.cfg │ ├── src │ │ ├── imu_filter_node.cpp │ │ ├── imu_filter_nodelet.cpp │ │ └── stateless_orientation.cpp │ ├── package.xml │ ├── CMakeLists.txt │ └── test │ │ ├── test_helpers.h │ │ ├── madgwick_test.cpp │ │ └── stateless_orientation_test.cpp ├── .travis.yml ├── .gitignore ├── imu_complementary_filter │ ├── launch │ │ ├── complementary_filter.launch │ │ └── complementary_filter.l │ ├── package.xml │ ├── CMakeLists.txt │ ├── src │ │ └── complementary_filter_node.cpp │ ├── CHANGELOG.rst │ └── include │ │ └── imu_complementary_filter │ │ └── complementary_filter_ros.h ├── Dockerfile-kinetic └── README.md ├── ROS_noetic └── rviz显示.png ├── serial_imu ├── src │ ├── packet.c │ ├── sub_spec.cpp │ ├── packet.h │ ├── imu_data_decode.h │ ├── sub_0x91.cpp │ ├── imu_data_decode.c │ ├── sub_0x62.cpp │ └── serial_imu.cpp ├── msg │ ├── Imu_0x91_msg.msg │ ├── Imu_0x62_msg.msg │ └── Imu_data_package.msg └── package.xml ├── racecar_driver ├── lib │ └── libracecar_driver.a ├── README ├── include │ └── racecar_driver.h ├── src │ └── racecar.cpp └── package.xml ├── imu_launch ├── launch │ ├── imu_rviz.launch │ ├── imu_0x91_msg.launch │ ├── imu_0x62_msg.launch │ ├── imu_spec_msg.launch │ ├── imu_display_3d.launch │ └── imu_msg.launch └── package.xml ├── encoder_driver ├── launch │ └── wheel_odom.launch ├── README.md ├── CMakeLists.txt ├── package.xml └── src │ ├── Encoder_vel.py │ └── imu_encoder_mix.cpp └── multi_goal ├── README.md └── src ├── test.csv ├── getpoints.py └── goal_loop.py /README.md: -------------------------------------------------------------------------------- 1 | # racecar 2 | 全国智能驾驶大赛暨全国大学生智能汽车竞赛室外专项赛 国家二等奖程序 3 | -------------------------------------------------------------------------------- /racecar/ssh.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ssh sz@192.168.5.101 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: src/doc 3 | -------------------------------------------------------------------------------- /racecar/map/test.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar/map/test.pgm -------------------------------------------------------------------------------- /racecar/udev/README: -------------------------------------------------------------------------------- 1 | #添加Racecar的USB设备,使用如下指令: 2 | sudo bash racecar_init.sh 3 | 然后重启电脑 4 | -------------------------------------------------------------------------------- /ROS_noetic/rviz显示.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/ROS_noetic/rviz显示.png -------------------------------------------------------------------------------- /racecar/map/mymap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar/map/mymap.pgm -------------------------------------------------------------------------------- /racecar/map/test.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar/map/test.yaml -------------------------------------------------------------------------------- /racecar/map/test1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar/map/test1.pgm -------------------------------------------------------------------------------- /serial_imu/src/packet.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/serial_imu/src/packet.c -------------------------------------------------------------------------------- /serial_imu/msg/Imu_0x91_msg.msg: -------------------------------------------------------------------------------- 1 | #90 91 data_package 2 | Header header 3 | Imu_data_package imu_data 4 | 5 | -------------------------------------------------------------------------------- /racecar/save_map.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun map_server map_saver -f ~/ROS-Autocar/src/racecar/map/mymap 4 | -------------------------------------------------------------------------------- /racecar/src/lib/libracecar_controller.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar/src/lib/libracecar_controller.a -------------------------------------------------------------------------------- /racecar_driver/lib/libracecar_driver.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/racecar_driver/lib/libracecar_driver.a -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/rviz_imu_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/imu_tool/rviz_imu_plugin/rviz_imu_plugin.png -------------------------------------------------------------------------------- /racecar/udev/car.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by arduino 2 | # 3 | KERNELS=="2-4:1.0", MODE:="666", SYMLINK+="car" 4 | 5 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/sample/ardrone_imu.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/imu_tool/imu_filter_madgwick/sample/ardrone_imu.bag -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/sample/sparkfun_razor.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/imu_tool/imu_filter_madgwick/sample/sparkfun_razor.bag -------------------------------------------------------------------------------- /racecar/udev/encoder.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by arduino 2 | # 3 | KERNELS=="2-6:1.0", MODE:="666", SYMLINK+="encoder" 4 | 5 | -------------------------------------------------------------------------------- /serial_imu/msg/Imu_0x62_msg.msg: -------------------------------------------------------------------------------- 1 | # 0x62 data_package 2 | Header header 3 | uint8 tag 4 | uint8 gw_id 5 | uint8 node_num 6 | Imu_data_package[16] node_data 7 | 8 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/sample/phidgets_imu_upside_down.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JiaNing-Z/racecar/HEAD/imu_tool/imu_filter_madgwick/sample/phidgets_imu_upside_down.bag -------------------------------------------------------------------------------- /racecar/map/test1.yaml: -------------------------------------------------------------------------------- 1 | image: test1.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /racecar/udev/laser.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by ls-lidar 2 | # 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001",MODE:="0666", SYMLINK+="laser" 4 | 5 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_0x91_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_0x62_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_spec_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /racecar/map/mymap.yaml: -------------------------------------------------------------------------------- 1 | image: /home/racecar/racecar/src/racecar/map/mymap.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_display_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /imu_launch/launch/imu_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /racecar/udev/racecar_init.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo cp ./car.rules /etc/udev/rules.d 3 | sudo cp ./laser.rules /etc/udev/rules.d 4 | sudo cp ./imu.rules /etc/udev/rules.d 5 | sudo cp ./encoder.rules /etc/udev/rules.d 6 | # 7 | exit 0 8 | -------------------------------------------------------------------------------- /encoder_driver/launch/wheel_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /racecar/launch/teleop_joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/include/imu_filter_madgwick/world_frame.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 2 | #define IMU_FILTER_MADGWICK_WORLD_FRAME_H_ 3 | 4 | namespace WorldFrame { 5 | enum WorldFrame { ENU, NED, NWU }; 6 | } 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /racecar/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /racecar/udev/imu.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by hi226 2 | # 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="666", SYMLINK+="imu" 4 | 5 | # set the udev rule , make the device_port be fixed by sanchi_imu 6 | # 7 | 8 | -------------------------------------------------------------------------------- /racecar/cfg/imu.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "racecar" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -10, 10) 8 | 9 | exit(gen.generate(PACKAGE, "racecar", "imu")) 10 | -------------------------------------------------------------------------------- /multi_goal/README.md: -------------------------------------------------------------------------------- 1 | ## Multi Goal ## 2 | #### 包内有两个代码文件 .\src\getpoints.py .\src\goal_loop.py 3 | 4 | ##### getpoints.py 5 | 该文件的主要功能为记录途经点，并将途经点保存至同目录下的test.csv中，运行该脚本时需要在终端中cd到multi_goal\src文件夹，在rviz中可以使用goal point选点，需要注意顺序，结束选点时，在脚本的终端内输入 f 即可 6 | 7 | ##### goal_loop.py 8 | 该文件的主要功能是根据车辆的位置进行多点循环导航。导航点读取的是test.csv中的数据，切换点的时间会有一点的提前，为了保证比较好的路径跟踪效果。 -------------------------------------------------------------------------------- /racecar/launch/car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /serial_imu/src/sub_spec.cpp: -------------------------------------------------------------------------------- 1 | //subscriber imu specifcation data package 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc,argv,"sub_spec"); 10 | ros::NodeHandle n; 11 | execlp("rostopic", "rostopic", "echo", "/IMU_data",NULL); 12 | 13 | ros::spin(); 14 | 15 | } 16 | -------------------------------------------------------------------------------- /imu_tool/.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | 3 | services: 4 | - docker 5 | 6 | env: 7 | matrix: 8 | - CI_ROS_DISTRO="kinetic" 9 | 10 | install: 11 | - docker build -t imu_tools_$CI_ROS_DISTRO -f Dockerfile-$CI_ROS_DISTRO . 12 | 13 | script: 14 | - docker run imu_tools_$CI_ROS_DISTRO /bin/bash -c "source devel/setup.bash && catkin run_tests && catkin_test_results" 15 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/imu_filter_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | Imu Filter nodelet publisher. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | Displays the orientation and acceleration components of sensor_msgs/Imu messages. 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /multi_goal/src/test.csv: -------------------------------------------------------------------------------- 1 | 3.802596092224121,2.1412878036499023,0.2882125757511959,0.957566452617708 2 | 2.6905717849731445,5.376918792724609,0.7896941641660048,0.6135007148179658 3 | 1.2487719058990479,8.993573188781738,0.9205451596762635,0.3906361593562514 4 | -1.7957332134246826,10.536282539367676,0.987439580364141,0.15799707317000827 5 | -6.061672210693359,9.39711856842041,-0.9997963389312687,0.02018119569380167 6 | -------------------------------------------------------------------------------- /racecar_driver/README: -------------------------------------------------------------------------------- 1 | /*******************************************************************/ 2 | /***************racecar_driver*****************************************/ 3 | /*******************************************************************/ 4 | 这是racecar的底层驱动例程 5 | 这个历程订阅/car/cmd_vel的twist的消息,发送给底层单片机 6 | 具体可以参考racecar.cpp 7 | 使用send_cmd()函数向单片机发送指令,函数参数分别为电机PWM和舵机PWM. 8 | PWM范围为500-2500us. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /serial_imu/msg/Imu_data_package.msg: -------------------------------------------------------------------------------- 1 | uint8 tag 2 | uint8 bitmap 3 | uint8 id 4 | float32 prs 5 | uint32 time 6 | uint32 frame_rate 7 | float32 acc_x 8 | float32 acc_y 9 | float32 acc_z 10 | float32 gyr_x 11 | float32 gyr_y 12 | float32 gyr_z 13 | float32 mag_x 14 | float32 mag_y 15 | float32 mag_z 16 | float32 eul_r 17 | float32 eul_p 18 | float32 eul_y 19 | float32 quat_w 20 | float32 quat_x 21 | float32 quat_y 22 | float32 quat_z 23 | -------------------------------------------------------------------------------- /racecar/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 1.0 8 | controller_patience: 3.0 9 | 10 | planner_frequency: 10.0 11 | planner_patience: 5.0 12 | 13 | oscillation_timeout: 10.0 14 | oscillation_distance: 0.2 15 | 16 | clearing_rotation_allowed: false 17 | -------------------------------------------------------------------------------- /imu_tool/.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .pydevproject 4 | cmake_install.cmake 5 | bin/ 6 | build/ 7 | lib/ 8 | *.cfgc 9 | imu_filter_madgwick/cfg/cpp/ 10 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig-usage.dox 11 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig.dox 12 | imu_filter_madgwick/docs/ImuFilterMadgwickConfig.wikidoc 13 | imu_filter_madgwick/src/imu_filter_madgwick/__init__.py 14 | imu_filter_madgwick/src/imu_filter_madgwick/cfg/ 15 | *~ -------------------------------------------------------------------------------- /racecar/racecar_rviz.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #for open rviz and set ROS master ip 4 | #Steven.Zhang 5 | #2018.03.09 6 | 7 | function get_ip_address { ifconfig | fgrep -v 127.0.0.1 | fgrep 'Mask:255.255.255.0' | egrep -o 'addr:[^ ]*' | fgrep '.8.'| sed 's/^.*://'; } 8 | 9 | #export ROS_IP=$(get_ip_address) 10 | export ROS_IP=`hostname -I` 11 | export ROS_MASTER_URI=http://192.168.5.101:11311 12 | 13 | ##source ../devel/setup.bash 14 | #rosrun rviz rviz 15 | -------------------------------------------------------------------------------- /racecar/launch/includes/car_tf.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /encoder_driver/README.md: -------------------------------------------------------------------------------- 1 | ## Encoder ## 2 | #### 包内有两个代码文件 .\src\Encoder_vel.py .\src\imu_encoder_mix.cpp 3 | ##### Encoder_vel.py 4 | 负责串口的打开与数据读取发布,主要参数为：port baud k 5 | racecar\launch\Run_car.launch中已经写入了该脚本，port 与 baud都不需要修改（新车需要绑定串口） 6 | k是滑移系数，需要在使用前进行标定，即让车行走1m，观察里程计话题(/encoder_imu_odom)的输出量为a，则k=1/a 7 | 8 | ##### imu_encoder_mix.cpp 9 | IMU 与编码器的融合节点，订阅的话题为/imu_data /encoder ，输出话题为/encoder_imu_odom， 如果里程计不工作先排查两个输入的话题是否正常发值。 10 | encoder_driver\launch\wheel_odom.launch 该文件可以单独启动轮式里程计 11 | -------------------------------------------------------------------------------- /racecar/launch/includes/rplidar.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /racecar_driver/include/racecar_driver.h: -------------------------------------------------------------------------------- 1 | 2 | // 3 | //racecar 4 | // 5 | 6 | #ifndef RACECAR_DRIVER 7 | #define RACECAR_DRIVER 8 | #include 9 | #include 10 | 11 | #if defined(__cplusplus) 12 | extern "C" { 13 | #endif 14 | 15 | 16 | 17 | 18 | int Open_Serial_Dev(char *dev); 19 | 20 | int art_racecar_init(int speed,char *dev);//设置波特率和串口设备 21 | 22 | 23 | unsigned char send_cmd(uint16_t motor_pwm,uint16_t servo_pwm);//发送指令(电机PWM,舵机PWM),单位为us. 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | #if defined(__cplusplus) 32 | } 33 | #endif 34 | #endif 35 | -------------------------------------------------------------------------------- /racecar/param/base_global_planner_params.yaml: -------------------------------------------------------------------------------- 1 | GlobalPlanner: 2 | allow_unknown: false 3 | default_tolerance: 0.5 #路径规划器目标点的公差范围 4 | # visualize_potential: true #指定是否通过可视化PointCloud2计算的潜在区域 5 | use_dijkstra: false #如果为ture，则使用dijkstra算法。 否则使用A *算法 6 | use_quadratic: true #二次逼近 7 | use_grid_path: false #如果为true，沿着栅格边界创建路径。 否则，使用梯度下降的方法。 8 | # old_navfn_behavior: true #如果你想要global_planner准确反映navfn的行为，此项设置为true。 9 | 10 | # lethal_cost: 25 11 | # neutral_cost: 50 12 | # cost_factor: 3 13 | # publish_potential: True 14 | 15 | 16 | NavfnROS: 17 | allow_unknown: false 18 | -------------------------------------------------------------------------------- /racecar/src/PID.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PID_H 3 | #define PID_H 4 | 5 | /***********************************/ 6 | typedef struct PID 7 | { //结构体定义 8 | double SetPoint; //设定值 9 | double Proportion; // Proportion 比例系数 10 | double Integral; // Integral 积分系数 11 | double Derivative; // Derivative 微分系数 12 | double LastError; // Error[-1] 前一拍误差 13 | double PreError; // Error[-2] 前两拍误差 14 | 15 | 16 | }PID; 17 | 18 | 19 | void PIDInit (struct PID *pp); 20 | double PIDCal(struct PID *pp, double ThisError); 21 | 22 | 23 | 24 | 25 | #endif 26 | 27 | 28 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/launch/complementary_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | #### Nodelet manager ###################################################### 4 | 5 | 6 | 7 | #### Complementary filter 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /serial_imu/src/packet.h: -------------------------------------------------------------------------------- 1 | #ifndef __PACKET_H__ 2 | #define __PACKET_H__ 3 | 4 | #include 5 | #include 6 | 7 | #define MAX_PACKET_LEN (2048) 8 | extern uint32_t frame_count; 9 | extern uint32_t crc_error_count; 10 | 11 | typedef struct 12 | { 13 | uint32_t ofs; 14 | uint8_t buf[MAX_PACKET_LEN]; /* total frame buffer */ 15 | uint16_t payload_len; 16 | uint16_t len; /* total frame len */ 17 | uint8_t type; 18 | }packet_t; 19 | 20 | 21 | /* packet Rx API */ 22 | typedef void (*on_data_received_event)(packet_t *pkt); 23 | void packet_decode_init(packet_t *pkt, on_data_received_event rx_handler); 24 | uint32_t packet_decode(uint8_t c); 25 | 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /imu_tool/Dockerfile-kinetic: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-ros-core 2 | 3 | RUN apt-get update \ 4 | && apt-get install -y build-essential python-catkin-tools python-rosdep \ 5 | && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* 6 | 7 | # Create ROS workspace 8 | COPY . /ws/src/imu_tools 9 | WORKDIR /ws 10 | 11 | # Use rosdep to install all dependencies (including ROS itself) 12 | RUN rosdep init && rosdep update && rosdep install --from-paths src -i -y --rosdistro kinetic 13 | 14 | RUN /bin/bash -c "source /opt/ros/kinetic/setup.bash && \ 15 | catkin init && \ 16 | catkin config --install -j 1 -p 1 && \ 17 | catkin build --limit-status-rate 0.1 --no-notify && \ 18 | catkin build --limit-status-rate 0.1 --no-notify --make-args tests" 19 | -------------------------------------------------------------------------------- /racecar/launch/ls01g_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | #设置激光数据topic名称 7 | #激光坐标 8 | #雷达连接的串口 9 | # 设置为true探测不到区域会变成最大值 10 | # true：探测不到区域为0，false：探测不到区域为inf 11 | # 角度制，从angle_disable_min到angle_disable_max之前的值为0 12 | 13 | # 如果0度方向在串口线的方向上设置为true 14 | 15 | 16 | -------------------------------------------------------------------------------- /racecar_driver/src/racecar.cpp: -------------------------------------------------------------------------------- 1 | // 2 | //racecar 3 | // 4 | 5 | #include "../include/racecar_driver.h" 6 | #include 7 | #include 8 | #include 9 | 10 | void TwistCallback(const geometry_msgs::Twist& twist) 11 | { 12 | double angle; 13 | //ROS_INFO("x= %f", twist.linear.x); 14 | //ROS_INFO("z= %f", twist.angular.z); 15 | angle = 2500.0 - twist.angular.z * 2000.0 / 180.0; 16 | //ROS_INFO("angle= %d",uint16_t(angle)); 17 | send_cmd(uint16_t(twist.linear.x),uint16_t(angle)); 18 | } 19 | 20 | int main(int argc, char** argv) 21 | { 22 | char data[] = "/dev/car"; 23 | art_racecar_init(38400,data); 24 | ros::init(argc, argv, "racecar_driver"); 25 | ros::NodeHandle n; 26 | 27 | ros::Subscriber sub = n.subscribe("/car/cmd_vel",1,TwistCallback); 28 | 29 | 30 | 31 | ros::spin(); 32 | 33 | } 34 | -------------------------------------------------------------------------------- /racecar/launch/includes/ls01g.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 　　 #设置激光数据topic名称 6 | 　　 #激光坐标 7 | 　　 #雷达连接的串口 8 | # 设置为true探测不到区域会变成最大值 9 | # true：探测不到区域为0，false：探测不到区域为inf 10 | # 角度制，从angle_disable_min到angle_disable_max之前的值为0 11 | 12 | # 如果0度方向在串口线的方向上设置为true 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /racecar/launch/ls01b_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | #设置激光数据topic名称 6 | #激光坐标 7 | #雷达连接的串口 8 | #雷达连接的串口波特率 9 | #雷达角度分辨率 10 | # 设置为true探测不到区域会变成最大值 11 | # true：探测不到区域为0，false：探测不到区域为inf 12 | #　雷达转速 13 | #　角度补偿，非同轴 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /racecar/param/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | acc_lim_th: 1 4 | acc_lim_x: 1 5 | acc_lim_y: 1 6 | 7 | 8 | max_vel_x: 1 9 | min_vel_x: 1 10 | 11 | max_vel_y: 1 12 | min_vel_y: 1 13 | 14 | max_vel_trans: 1 15 | min_vel_trans: 1 16 | max_rot_vel: 1 17 | min_rot_vel: 1 18 | 19 | sim_time: 1.5 20 | 21 | sim_granularity: 0.1 22 | 23 | goal_distance_bias: 30.0 24 | path_distance_bias: 20.0 25 | occdist_scale: 0.01 26 | forward_point_distance: 0.5 27 | stop_time_buffer: 0.2 28 | ####### 29 | scaling_speed: 0.5 30 | ####### 31 | max_scaling_factor: 0.2 32 | oscillation_reset_dist: 0.05 33 | 34 | # 35 | vx_samples: 20 36 | vy_samples: 0 37 | vtheta_samples: 10 38 | 39 | theta_stopped_vel: 0.01 40 | trans_stopped_vel: 0.01 41 | # 42 | xy_goal_tolerance: 0.2 43 | yaw_goal_tolerance: 0.1 44 | latch_xy_goal_tolerance: true #如果目标误差被锁定，若机器人达到目标XY位置，它将旋转到位，即使误差没有达到，也会做旋转 45 | -------------------------------------------------------------------------------- /racecar/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | footprint: [[-0.305, -0.18], [-0.305, 0.18], [0.305, 0.18], [0.305, -0.18]] 3 | footprint_padding: 0.01 4 | transform_tolerance: 0.5 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | 8 | global_frame: map 9 | robot_base_frame: base_footprint 10 | resolution: 0.10 11 | 12 | rolling_window: true 13 | width: 10.0 14 | height: 10.0 15 | resolution: 0.05 16 | 17 | track_unknown_space: false 18 | 19 | plugins: 20 | - {name: sensor, type: "costmap_2d::ObstacleLayer"} 21 | - {name: inflation, type: "costmap_2d::InflationLayer"} 22 | 23 | sensor: 24 | observation_sources: laser_scan_sensor 25 | laser_scan_sensor: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true} 26 | 27 | inflation: 28 | inflation_radius: 0.1 29 | cost_scaling_factor: 8.0 30 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_imu_plugin 3 | 1.1.8 4 | 5 | RVIZ plugin for IMU visualization 6 | 7 | BSD 8 | http://ros.org/wiki/rviz_imu_plugin 9 | Ivan Dryanovski 10 | Martin Günther 11 | 12 | catkin 13 | 14 | qtbase5-dev 15 | roscpp 16 | rviz 17 | 18 | libqt5-core 19 | libqt5-gui 20 | libqt5-widgets 21 | roscpp 22 | rviz 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /racecar/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | footprint: [[-0.305, -0.18], [-0.305, 0.18], [0.305, 0.18], [0.305, -0.18]] 3 | footprint_padding: 0.01 4 | transform_tolerance: 0.5 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | 8 | global_frame: map 9 | robot_base_frame: base_footprint 10 | resolution: 0.10 11 | 12 | rolling_window: true 13 | width: 20.0 14 | height: 20.0 15 | track_unknown_space: false 16 | 17 | plugins: 18 | - {name: static, type: "costmap_2d::StaticLayer"} 19 | - {name: sensor, type: "costmap_2d::ObstacleLayer"} 20 | - {name: inflation, type: "costmap_2d::InflationLayer"} 21 | 22 | static: 23 | map_topic: /map 24 | subscribe_to_updates: true 25 | 26 | sensor: 27 | observation_sources: laser_scan_sensor 28 | laser_scan_sensor: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true} 29 | 30 | inflation: 31 | inflation_radius: 0.1 32 | cost_scaling_factor: 8.0 33 | 34 | -------------------------------------------------------------------------------- /racecar/launch/Run_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # IMU Filter dynamic reconfigure 3 | 4 | PACKAGE='imu_filter_madgwick' 5 | 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("gain", double_t, 0, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal.", 0.1, 0.0, 1.0) 11 | gen.add("zeta", double_t, 0, "Gyro drift gain (approx. rad/s).", 0, -1.0, 1.0) 12 | gen.add("mag_bias_x", double_t, 0, "Magnetometer bias (hard iron correction), x component.", 0, -10.0, 10.0) 13 | gen.add("mag_bias_y", double_t, 0, "Magnetometer bias (hard iron correction), y component.", 0, -10.0, 10.0) 14 | gen.add("mag_bias_z", double_t, 0, "Magnetometer bias (hard iron correction), z component.", 0, -10.0, 10.0) 15 | gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 1.0) 16 | 17 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ImuFilterMadgwick")) 18 | 19 | -------------------------------------------------------------------------------- /encoder_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(encoder_driver) 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 | rospy 12 | roscpp 13 | serial 14 | std_msgs 15 | nav_msgs 16 | std_msgs 17 | geometry_msgs 18 | tf 19 | dynamic_reconfigure 20 | ) 21 | 22 | catkin_package( 23 | LIBRARIES art_encoder_driver 24 | CATKIN_DEPENDS 25 | rospy 26 | std_msgs 27 | serial 28 | geometry_msgs 29 | tf 30 | ) 31 | 32 | include_directories( 33 | # include 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | 38 | 39 | add_executable(${PROJECT_NAME}_node1 40 | src/imu_encoder_mix.cpp) 41 | add_dependencies(${PROJECT_NAME}_node1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 42 | target_link_libraries(${PROJECT_NAME}_node1 43 | ${catkin_LIBRARIES} 44 | ) 45 | 46 | 47 | 48 | SET( CMAKE_CXX_FLAGS "-std=c++11 -O3") 49 | 50 | 51 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/launch/complementary_filter.l: -------------------------------------------------------------------------------- 1 | 2 | 3 | #### Nodelet manager ###################################################### 4 | 5 | 7 | 8 | #### IMU Driver ########################################################### 9 | 10 | 13 | 14 | # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms) 15 | 16 | 17 | 18 | 19 | #### Complementary filter 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/src/imu_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/imu_filter_ros.h" 26 | 27 | int main (int argc, char **argv) 28 | { 29 | ros::init (argc, argv, "ImuFilter"); 30 | ros::NodeHandle nh; 31 | ros::NodeHandle nh_private("~"); 32 | ImuFilterRos imu_filter(nh, nh_private); 33 | ros::spin(); 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_complementary_filter 4 | 1.1.8 5 | Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into a quaternion to represent the orientation of the device wrt the global frame. Based on the algorithm by Roberto G. Valenti etal. described in the paper "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs" available at http://www.mdpi.com/1424-8220/15/8/19302 . 6 | 7 | Roberto G. Valenti 8 | BSD 9 | 10 | http://www.mdpi.com/1424-8220/15/8/19302 11 | Roberto G. Valenti 12 | 13 | catkin 14 | cmake_modules 15 | message_filters 16 | roscpp 17 | sensor_msgs 18 | std_msgs 19 | tf 20 | message_filters 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | 26 | -------------------------------------------------------------------------------- /racecar/launch/includes/rf2o.launch.xml: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 26 | #define IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 27 | 28 | #include 29 | 30 | #include "imu_filter_madgwick/imu_filter_ros.h" 31 | 32 | class ImuFilterNodelet : public nodelet::Nodelet 33 | { 34 | public: 35 | virtual void onInit(); 36 | 37 | private: 38 | boost::shared_ptr filter_; 39 | }; 40 | 41 | #endif // IMU_FILTER_MADGWICK_IMU_FILTER_NODELET_H 42 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/src/imu_filter_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/imu_filter_nodelet.h" 26 | #include 27 | 28 | void ImuFilterNodelet::onInit() 29 | { 30 | NODELET_INFO("Initializing IMU Filter Nodelet"); 31 | 32 | // TODO: Do we want the single threaded or multithreaded NH? 33 | ros::NodeHandle nh = getMTNodeHandle(); 34 | ros::NodeHandle nh_private = getMTPrivateNodeHandle(); 35 | 36 | filter_.reset(new ImuFilterRos(nh, nh_private)); 37 | } 38 | 39 | PLUGINLIB_EXPORT_CLASS(ImuFilterNodelet, nodelet::Nodelet) 40 | -------------------------------------------------------------------------------- /racecar/README.md: -------------------------------------------------------------------------------- 1 | #Racecar 2 | 3 | ROS racecar 4 | ************************安装************************ 5 | 6 | 配置小车串口udev： 7 | cd ~/racecar/src/racecar/udev 8 | sudo bash racecar_init.sh 9 | sudo reboot 10 | **********************建立地图********************** 11 | 先安装电脑用户名和主机名配置主从机 12 | a) 运行车 13 | roslaunch racecar Run_car.launch 14 | b) 3.3运行gmapping 15 | roslaunch racecar Run_gmapping.launch 16 | c) 3.4运行键盘控制 17 | rosrun racecar racecar_teleop.py 18 | 或者手柄遥控 19 | roslaunch racecar teleop_joy.launch 20 | d) 3.6 建立地图 21 | 键盘控制建立地图,按键如下： 22 | U I O 23 | J K L 24 | M , . 25 | 加减速为W，S. 26 | 手柄控制： 27 | L侧方向键:上下为加速减速，左右为转向幅度大小调节 28 | R侧摇杆：控制小车运动 29 | e) 保存地图（地图直接保存在小车上） 30 | 在racecar文件夹下执行：bash save_map.sh 31 | 地图保存在racecar/map/mymap.pgm 32 | 33 | 34 | ************************导航************************ 35 | 36 | a) 运行车 37 | roslaunch racecar Run_car.launch 38 | b) 运行AMCL 39 | roslaunch racecar amcl_nav.launch 40 | c) 4.5 开始导航 41 | 在RVIZ中设定初始坐标，设定目标位置，开始导航 42 | *********************软件接口*********************** 43 | 1.启动底盘 44 | 启动底盘需要启动rosserial_python节点。 45 | 设置参考racecar/launch/Run_car.launch 46 | 2.发布地盘控制指令： 47 | 通过发布Twist消息控制底盘。 48 | 线速度：twist.linear.x,这里的线速度范围为500～2500（对应PWM脉冲为0.5ms～2.5ms）,1500为静止，1500-2500为正向速度，500-1500为反向速度。 49 | 角速度：twist.angular.x，这里角度范围为0～180度,90度为中间值，90-180度左转，0-90度右转。 50 | 3.里程计数据： 51 | 里程计采用激光雷达和IMU数据融合的里程计。 52 | 需要先启动IMU节点和雷达节点，参考racecar/launch/Run_car.launch 53 | 然后启动rf2o节点，用rf2o生成激光里程计，参考racecar/launch/includes/rf2o.launch.xml 54 | 再启动robot_localization用EKF融合里程计信息，参考racecar/launch/Run_gmapping.launch 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | imu_filter_madgwick 3 | 1.1.8 4 | 5 | Filter which fuses angular velocities, accelerations, and (optionally) magnetic readings from a generic IMU device into an orientation. Based on code by Sebastian Madgwick, http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms. 6 | 7 | GPL 8 | http://ros.org/wiki/imu_filter_madgwick 9 | Ivan Dryanovski 10 | Martin Günther 11 | 12 | catkin 13 | 14 | roscpp 15 | sensor_msgs 16 | geometry_msgs 17 | tf2 18 | tf2_geometry_msgs 19 | tf2_ros 20 | nodelet 21 | pluginlib 22 | message_filters 23 | dynamic_reconfigure 24 | 25 | roscpp 26 | sensor_msgs 27 | geometry_msgs 28 | tf2 29 | tf2_geometry_msgs 30 | tf2_ros 31 | nodelet 32 | pluginlib 33 | message_filters 34 | dynamic_reconfigure 35 | 36 | rosunit 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 26 | #define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | class StatelessOrientation 33 | { 34 | public: 35 | static bool computeOrientation( 36 | WorldFrame::WorldFrame frame, 37 | geometry_msgs::Vector3 acceleration, 38 | geometry_msgs::Vector3 magneticField, 39 | geometry_msgs::Quaternion& orientation); 40 | 41 | static bool computeOrientation( 42 | WorldFrame::WorldFrame frame, 43 | geometry_msgs::Vector3 acceleration, 44 | geometry_msgs::Quaternion& orientation); 45 | 46 | }; 47 | 48 | #endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H 49 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | cmake_policy(SET CMP0048 NEW) 3 | project(imu_complementary_filter) 4 | 5 | find_package(Boost REQUIRED COMPONENTS thread) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | cmake_modules 9 | message_filters 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | tf 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | LIBRARIES complementary_filter 19 | CATKIN_DEPENDS message_filters roscpp sensor_msgs std_msgs tf 20 | ) 21 | 22 | include_directories( 23 | include 24 | ${catkin_INCLUDE_DIRS} 25 | ${Boost_INCLUDE_DIRS} 26 | ) 27 | 28 | ## Declare a cpp library 29 | add_library(complementary_filter 30 | src/complementary_filter.cpp 31 | src/complementary_filter_ros.cpp 32 | include/imu_complementary_filter/complementary_filter.h 33 | include/imu_complementary_filter/complementary_filter_ros.h 34 | ) 35 | target_link_libraries(complementary_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 36 | 37 | 38 | # create complementary_filter_node executable 39 | add_executable(complementary_filter_node 40 | src/complementary_filter_node.cpp) 41 | target_link_libraries(complementary_filter_node complementary_filter ${catkin_LIBRARIES}) 42 | 43 | install(TARGETS complementary_filter 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 47 | ) 48 | 49 | install(TARGETS complementary_filter_node 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | 55 | ## Mark cpp header files for installation 56 | install(DIRECTORY include/${PROJECT_NAME}/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | FILES_MATCHING PATTERN "*.h" 59 | PATTERN ".svn" EXCLUDE 60 | ) 61 | -------------------------------------------------------------------------------- /serial_imu/src/imu_data_decode.h: -------------------------------------------------------------------------------- 1 | #ifndef __IMU_DATA_DECODE_H__ 2 | #define __IMU_DATA_DECODE_H__ 3 | 4 | #include 5 | #include 6 | #define MAX_LENGTH 16 7 | 8 | extern uint8_t bitmap; 9 | #define BIT_VALID_ID (0x01) 10 | #define BIT_VALID_ACC (0x02) 11 | #define BIT_VALID_GYR (0x04) 12 | #define BIT_VALID_MAG (0x08) 13 | #define BIT_VALID_EUL (0x10) 14 | #define BIT_VALID_QUAT (0x20) 15 | #define BIT_VALID_TIME (0x40) 16 | #define BIT_VALID_PRS (0x80) 17 | #define BIT_VALID_ALL (BIT_VALID_QUAT | BIT_VALID_EUL | BIT_VALID_MAG | BIT_VALID_GYR | BIT_VALID_ACC | BIT_VALID_ID | BIT_VALID_TIME | BIT_VALID_PRS) 18 | 19 | typedef struct id0x91_t { 20 | uint8_t tag; 21 | uint8_t id; 22 | uint8_t rev[2]; 23 | float prs; 24 | uint32_t time; 25 | float acc[3]; 26 | float gyr[3]; 27 | float mag[3]; 28 | float eul[3]; 29 | float quat[4]; 30 | 31 | }__attribute__((packed())) id0x91_t; 32 | 33 | typedef struct id0x62_t { 34 | uint8_t tag; 35 | uint8_t gw_id; 36 | uint8_t n; 37 | uint8_t rev[5]; 38 | id0x91_t id0x91[MAX_LENGTH]; 39 | }__attribute__((packed())) id0x62_t; 40 | 41 | extern id0x91_t id0x91; 42 | extern id0x62_t id0x62; 43 | 44 | typedef enum 45 | { 46 | kItemID = 0x90, /* user programed ID */ 47 | kItemAccRaw = 0xA0, /* raw acc */ 48 | kItemGyrRaw = 0xB0, /* raw gyro */ 49 | kItemGyrRaw_yunjing = 0xB1, /* raw gyro yunjing */ 50 | kItemMagRaw = 0xC0, /* raw mag */ 51 | kItemRotationEul = 0xD0, /* eular angle */ 52 | kItemRotationQuat = 0xD1, /* att q */ 53 | kItemPressure = 0xF0, /* pressure */ 54 | kItemEnd = 0x00, 55 | KItemIMUSOL = 0x91, /* IMUSOL */ 56 | KItemGWSOL = 0x62, /* RFSOL */ 57 | }ItemID_t; 58 | 59 | int imu_data_decode_init(void); 60 | 61 | #endif 62 | 63 | 64 | -------------------------------------------------------------------------------- /racecar/src/car_controller.hpp: -------------------------------------------------------------------------------- 1 | /********************/ 2 | /* CLASS DEFINITION */ 3 | /********************/ 4 | class L1Controller 5 | { 6 | public: 7 | L1Controller(); 8 | void initMarker(); 9 | bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose); 10 | bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos); 11 | double getYawFromPose(const geometry_msgs::Pose& carPose); 12 | double getEta(const geometry_msgs::Pose& carPose); 13 | double getCar2GoalDist(); 14 | double getL1Distance(const double& _Vcmd); 15 | double getSteeringAngle(double eta); 16 | double getGasInput(const float& current_v); 17 | geometry_msgs::Point get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose); 18 | void PID_init(); 19 | 20 | private: 21 | struct PID *speed_pid; 22 | ros::NodeHandle n_; 23 | ros::Subscriber odom_sub, path_sub, goal_sub; 24 | ros::Publisher pub_, marker_pub; 25 | ros::Timer timer1, timer2; 26 | tf::TransformListener tf_listener; 27 | 28 | visualization_msgs::Marker points, line_strip, goal_circle; 29 | geometry_msgs::Twist cmd_vel; 30 | geometry_msgs::Point odom_goal_pos; 31 | nav_msgs::Odometry odom; 32 | nav_msgs::Path map_path, odom_path; 33 | 34 | double L, Lfw, Lrv, Vcmd, lfw, lrv, steering, u, v; 35 | double Gas_gain, baseAngle, Angle_gain, goalRadius; 36 | int controller_freq, baseSpeed; 37 | bool foundForwardPt, goal_received, goal_reached; 38 | int car_stop; 39 | 40 | void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg); 41 | void pathCB(const nav_msgs::Path::ConstPtr& pathMsg); 42 | void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg); 43 | void goalReachingCB(const ros::TimerEvent&); 44 | void controlLoopCB(const ros::TimerEvent&); 45 | 46 | 47 | 48 | }; // end of class 49 | 50 | -------------------------------------------------------------------------------- /multi_goal/src/getpoints.py: -------------------------------------------------------------------------------- 1 | #get the point and ore form rviz to txt 2 | #Type: geometry_msgs/PoseStamped /move_base_simple/goal 3 | #todo: change the count to the key "finish" 4 | #todo: not the cover the last csv 5 | import rospy 6 | import math 7 | import sys,select,termios,tty 8 | import tf 9 | import csv 10 | import codecs 11 | from geometry_msgs.msg import PoseStamped 12 | 13 | count = 0 14 | point=[] 15 | def PoseStampedCB(data): 16 | global count 17 | gx = data.pose.position.x 18 | gy = data.pose.position.y 19 | gz = data.pose.orientation.z 20 | gw = data.pose.orientation.w 21 | #count = count + 1 22 | point.append([gx,gy,gz,gw]) 23 | if(count == 3): 24 | data_write_csv('test.csv',point) 25 | # with open('test.csv','wb') as csvfile: 26 | # pointwriter = csv.writer(csvfile,delimiter=' ',quotechar='|',quoting=csv.QUOTE_MINIMAL) 27 | # pointwriter.writerow([gx,gy,gz,gw]) 28 | # print("write succ!!") 29 | 30 | def getKey(): 31 | tty.setraw(sys.stdin.fileno()) 32 | rlist,_,_ = select.select([sys.stdin],[],[],0.1) 33 | if rlist: 34 | key = sys.stdin.read(1) 35 | else: 36 | key = '' 37 | termios.tcsetattr(sys.stdin,termios.TCSADRAIN,settings) 38 | return key 39 | 40 | def data_write_csv(file_name, datas): 41 | file_csv = codecs.open(file_name,'w+','utf-8') 42 | writer = csv.writer(file_csv, delimiter=',', quoting=csv.QUOTE_MINIMAL) 43 | for data in datas: 44 | writer.writerow(data) 45 | print("write succ!!") 46 | 47 | if __name__ == '__main__': 48 | try: 49 | rospy.init_node("getPoint",anonymous=True) 50 | rospy.Subscriber('/move_base_simple/goal',PoseStamped,PoseStampedCB,queue_size=10) 51 | settings = termios.tcgetattr(sys.stdin) 52 | while(1): 53 | Key = getKey() 54 | if Key == 'f' or Key == 'F': 55 | data_write_csv('test.csv',point) 56 | if Key == '\x03': 57 | break 58 | rospy.spin() 59 | except rospy.ROSInterruptException: 60 | pass 61 | -------------------------------------------------------------------------------- /imu_tool/README.md: -------------------------------------------------------------------------------- 1 | IMU tools for ROS 2 | =================================== 3 | 4 | Overview 5 | ----------------------------------- 6 | 7 | IMU-related filters and visualizers. The stack contains: 8 | 9 | * `imu_filter_madgwick`: a filter which fuses angular velocities, 10 | accelerations, and (optionally) magnetic readings from a generic IMU 11 | device into an orientation. Based on the work of [1]. 12 | 13 | * `imu_complementary_filter`: a filter which fuses angular velocities, 14 | accelerations, and (optionally) magnetic readings from a generic IMU 15 | device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2]. 16 | 17 | * `rviz_imu_plugin` a plugin for rviz which displays `sensor_msgs::Imu` 18 | messages 19 | 20 | Installing 21 | ----------------------------------- 22 | 23 | ### From source ### 24 | 25 | [Create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) 26 | (e.g., `~/ros-hydro-ws/`) and source the `devel/setup.bash` file. 27 | 28 | Make sure you have git installed: 29 | 30 | sudo apt-get install git-core 31 | 32 | Download the stack from our repository into your catkin workspace (e.g., 33 | `ros-hydro-ws/src`; use the proper branch for your distro, e.g., `groovy`, 34 | `hydro`...): 35 | 36 | git clone -b https://github.com/ccny-ros-pkg/imu_tools.git 37 | 38 | Install any dependencies using [rosdep](http://www.ros.org/wiki/rosdep). 39 | 40 | rosdep install imu_tools 41 | 42 | Compile the stack: 43 | 44 | cd ~/ros-hydro-ws 45 | catkin_make 46 | 47 | More info 48 | ----------------------------------- 49 | 50 | http://wiki.ros.org/imu_tools 51 | 52 | License 53 | ----------------------------------- 54 | 55 | * `imu_filter_madgwick`: currently licensed as GPL, following the original implementation 56 | 57 | * `imu_complementary_filter`: BSD 58 | 59 | * `rviz_imu_plugin`: BSD 60 | 61 | References 62 | ----------------------------------- 63 | [1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 64 | 65 | [2] http://www.mdpi.com/1424-8220/15/8/19302 66 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/src/complementary_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 1. Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | 2. Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 3. Neither the name of the City College of New York nor the 17 | names of its contributors may be used to endorse or promote products 18 | derived from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "imu_complementary_filter/complementary_filter_ros.h" 33 | 34 | int main (int argc, char **argv) 35 | { 36 | ros::init (argc, argv, "ComplementaryFilterROS"); 37 | ros::NodeHandle nh; 38 | ros::NodeHandle nh_private("~"); 39 | imu_tools::ComplementaryFilterROS filter(nh, nh_private); 40 | ros::spin(); 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /racecar/launch/amcl_nav_no_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /serial_imu/src/sub_0x91.cpp: -------------------------------------------------------------------------------- 1 | //subscriiber 0x91 data package 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "imu_data_decode.h" 8 | 9 | void imu_0x91_callback(const serial_imu::Imu_0x91_msg imu_0x91_msg); 10 | 11 | int main(int argc,char **argv) 12 | { 13 | ros::init(argc,argv,"sub_0x91"); 14 | 15 | ros::NodeHandle n; 16 | 17 | ros::Subscriber imu_0x91_sub = n.subscribe("/imu_0x91_package", 10, imu_0x91_callback); 18 | 19 | ros::spin(); 20 | } 21 | 22 | 23 | 24 | 25 | void imu_0x91_callback(const serial_imu::Imu_0x91_msg imu_0x91_msg) 26 | { 27 | printf("\033c"); 28 | 29 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_ID) 30 | printf(" Devie ID:%6d\n",imu_0x91_msg.imu_data.id); 31 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_PRS) 32 | printf(" Prs(hPa): %6f\n", imu_0x91_msg.imu_data.prs); 33 | 34 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_TIME) 35 | printf(" Run times: %d days %d:%d:%d:%d\n",imu_0x91_msg.imu_data.time / 86400000, imu_0x91_msg.imu_data.time / 3600000 % 24, imu_0x91_msg.imu_data.time / 60000 % 60, imu_0x91_msg.imu_data.time / 1000 % 60, imu_0x91_msg.imu_data.time % 1000); 36 | 37 | printf(" Frame Rate: %4dHz\r\n", imu_0x91_msg.imu_data.frame_rate); 38 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_ACC) 39 | printf(" Acc(G):%8.3f %8.3f %8.3f\r\n", imu_0x91_msg.imu_data.acc_x, imu_0x91_msg.imu_data.acc_y, imu_0x91_msg.imu_data.acc_z); 40 | 41 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_GYR) 42 | printf(" Gyr(deg/s):%8.2f %8.2f %8.2f\r\n", imu_0x91_msg.imu_data.gyr_x, imu_0x91_msg.imu_data.gyr_y, imu_0x91_msg.imu_data.gyr_z); 43 | 44 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_MAG) 45 | printf(" Mag(uT):%8.2f %8.2f %8.2f\r\n", imu_0x91_msg.imu_data.mag_x, imu_0x91_msg.imu_data.mag_y, imu_0x91_msg.imu_data.mag_z); 46 | 47 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_EUL) 48 | printf(" Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x91_msg.imu_data.eul_r, imu_0x91_msg.imu_data.eul_p, imu_0x91_msg.imu_data.eul_y); 49 | 50 | if(imu_0x91_msg.imu_data.bitmap & BIT_VALID_QUAT) 51 | printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu_0x91_msg.imu_data.quat_w, imu_0x91_msg.imu_data.quat_x, imu_0x91_msg.imu_data.quat_y, imu_0x91_msg.imu_data.quat_z); 52 | } 53 | -------------------------------------------------------------------------------- /racecar/launch/Run_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /racecar/src/ESC_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | 7 | import sys, select, termios, tty 8 | import time 9 | 10 | msg = "" 11 | 12 | 13 | def getKey(): 14 | tty.setraw(sys.stdin.fileno()) 15 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 16 | if rlist: 17 | key = sys.stdin.read(1) 18 | else: 19 | key = '' 20 | 21 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 22 | return key 23 | 24 | 25 | def vels(speed,turn): 26 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 27 | 28 | if __name__=="__main__": 29 | settings = termios.tcgetattr(sys.stdin) 30 | 31 | rospy.init_node('racecar_teleop') 32 | pub = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5) 33 | 34 | 35 | run = 0 36 | 37 | try: 38 | while(1): 39 | key = getKey() 40 | twist = Twist() 41 | if key == '1': 42 | twist.linear.x = 1500; twist.linear.y = 0; 43 | twist.linear.z = 0 44 | twist.angular.x = 0; 45 | twist.angular.y = 0; 46 | twist.angular.z = 90 47 | print ("vel = 0") 48 | elif key == '2': 49 | twist.linear.x = 2500; twist.linear.y = 0; twist.linear.z = 0; 50 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 90; 51 | print ("vel = +Max") 52 | elif key == '3': 53 | twist.linear.x = 500; twist.linear.y = 0; twist.linear.z = 0; 54 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 90; 55 | print ("vel = -Max") 56 | else: 57 | twist.linear.x = 1500; twist.linear.y = 0; twist.linear.z = 0; 58 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 90; 59 | print ("stop") 60 | 61 | pub.publish(twist) 62 | 63 | 64 | 65 | if (key == '\x03'): #for ctrl + c exit 66 | break 67 | 68 | except: 69 | print ("error") 70 | 71 | finally: 72 | twist = Twist() 73 | twist.linear.x = speed_mid; twist.linear.y = 0; twist.linear.z = 0 74 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn_mid 75 | pub.publish(twist) 76 | 77 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 78 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | cmake_policy(SET CMP0048 NEW) 3 | project(rviz_imu_plugin) 4 | 5 | find_package(catkin REQUIRED COMPONENTS rviz) 6 | 7 | ## This setting causes Qt's "MOC" generation to happen automatically. 8 | set(CMAKE_AUTOMOC ON) 9 | 10 | ## This plugin includes Qt widgets, so we must include Qt. 11 | ## We'll use the version that rviz used so they are compatible. 12 | if(rviz_QT_VERSION VERSION_LESS "5") 13 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 14 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 15 | include(${QT_USE_FILE}) 16 | else() 17 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 18 | find_package(Qt5Widgets REQUIRED) 19 | find_package(Qt5Core REQUIRED) 20 | find_package(Qt5OpenGL REQUIRED) 21 | ## Set the QT_LIBRARIES variable for Qt5, so it can be used below. 22 | set(QT_LIBRARIES Qt5::Widgets) 23 | endif() 24 | 25 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 26 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 27 | add_definitions(-DQT_NO_KEYWORDS) 28 | 29 | catkin_package( 30 | DEPENDS 31 | CATKIN_DEPENDS rviz 32 | INCLUDE_DIRS 33 | LIBRARIES 34 | ) 35 | 36 | include_directories( 37 | include 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | ## Here we specify the list of source files. 42 | ## The generated MOC files are included automatically as headers. 43 | set(SRC_FILES src/imu_display.cpp 44 | src/imu_orientation_visual.cpp 45 | src/imu_axes_visual.cpp 46 | src/imu_acc_visual.cpp) 47 | 48 | ## Build libraries 49 | add_library(${PROJECT_NAME} ${SRC_FILES}) 50 | 51 | set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON) 52 | 53 | ## Link the library with whatever Qt libraries have been defined by 54 | ## the ``find_package(Qt4 ...)`` line above, or by the 55 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 56 | ## catkin has included. 57 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 58 | 59 | install(TARGETS 60 | ${PROJECT_NAME} 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | ) 65 | 66 | install(FILES plugin_description.xml 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 68 | -------------------------------------------------------------------------------- /racecar/launch/includes/default_mapping.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | cmake_policy(SET CMP0048 NEW) 3 | project(imu_filter_madgwick) 4 | 5 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs tf2 tf2_geometry_msgs tf2_ros nodelet pluginlib message_filters dynamic_reconfigure) 6 | 7 | find_package(Boost REQUIRED COMPONENTS system thread) 8 | 9 | # Generate dynamic parameters 10 | generate_dynamic_reconfigure_options(cfg/ImuFilterMadgwick.cfg) 11 | 12 | 13 | catkin_package( 14 | DEPENDS Boost 15 | CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs tf2_ros tf2_geometry_msgs nodelet pluginlib message_filters dynamic_reconfigure 16 | INCLUDE_DIRS 17 | LIBRARIES imu_filter imu_filter_nodelet 18 | ) 19 | 20 | include_directories( 21 | include 22 | ${catkin_INCLUDE_DIRS} 23 | ${Boost_INCLUDE_DIRS} 24 | ) 25 | 26 | 27 | # create imu_filter library 28 | add_library (imu_filter src/imu_filter.cpp src/imu_filter_ros.cpp src/stateless_orientation.cpp) 29 | add_dependencies(imu_filter ${PROJECT_NAME}_gencfg) 30 | target_link_libraries(imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 31 | 32 | # create imu_filter_nodelet library 33 | add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp) 34 | add_dependencies(imu_filter_nodelet ${PROJECT_NAME}_gencfg) 35 | target_link_libraries(imu_filter_nodelet imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 36 | 37 | # create imu_filter_node executable 38 | add_executable(imu_filter_node src/imu_filter_node.cpp) 39 | add_dependencies(imu_filter_node ${PROJECT_NAME}_gencfg) 40 | target_link_libraries(imu_filter_node imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 41 | 42 | 43 | install(TARGETS imu_filter_node 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 47 | ) 48 | 49 | install(TARGETS imu_filter imu_filter_nodelet 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 53 | ) 54 | 55 | install(DIRECTORY include/${PROJECT_NAME}/ 56 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 57 | FILES_MATCHING PATTERN "*.h" 58 | ) 59 | 60 | install(FILES imu_filter_nodelet.xml 61 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 62 | ) 63 | 64 | 65 | 66 | if(CATKIN_ENABLE_TESTING) 67 | catkin_add_gtest(${PROJECT_NAME}-madgwick_test 68 | test/stateless_orientation_test.cpp 69 | test/madgwick_test.cpp 70 | ) 71 | target_link_libraries(${PROJECT_NAME}-madgwick_test 72 | imu_filter 73 | ${catkin_LIBRARIES} 74 | ) 75 | endif() 76 | -------------------------------------------------------------------------------- /racecar/launch/includes/amcl.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /imu_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_launch 4 | 0.0.0 5 | The imu_launch package 6 | 7 | 8 | 9 | 10 | linux 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_imu_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.8 (2020-05-25) 6 | ------------------ 7 | * Export symbols so plugin can load 8 | * properly show/hide visualization when enabled/disabled 9 | * Contributors: Lou Amadio, v4hn 10 | 11 | 1.1.7 (2019-05-06) 12 | ------------------ 13 | * Fix includes, typos and log messages 14 | * print ros_warn and give unit quaternion to ogre to prevent rviz crash (`#90 `_) 15 | * Contributors: Jackey-Huo, Martin Günther 16 | 17 | 1.1.6 (2018-05-25) 18 | ------------------ 19 | 20 | 1.1.4 (2017-05-22) 21 | ------------------ 22 | * Add option to display orientation in world frame (`#69 `_) 23 | Per REP 145 IMU orientation is in the world frame. Rotating the 24 | orientation data to transform into the sensor frame results in strange 25 | behavior, such as double-rotation of orientation on a robot. Provide an 26 | option to display orientation in the world frame, and enable it by 27 | default. Continue to translate the position of the data to the sensor 28 | frame. 29 | * Contributors: C. Andy Martin 30 | 31 | 1.1.3 (2017-03-10) 32 | ------------------ 33 | 34 | 1.1.2 (2016-09-07) 35 | ------------------ 36 | 37 | 1.1.1 (2016-09-07) 38 | ------------------ 39 | 40 | 1.1.0 (2016-04-25) 41 | ------------------ 42 | * Add qt5 dependencies to rviz_imu_plugin package.xml 43 | This fixes the compilation errors on Kinetic for Debian Jessie. 44 | * Contributors: Martin Guenther 45 | 46 | 1.0.11 (2016-04-22) 47 | ------------------- 48 | 49 | 1.0.10 (2016-04-22) 50 | ------------------- 51 | * Support qt4/qt5 using rviz's exported qt version 52 | Closes `#58 `_ . 53 | This fixes the build on Kinetic, where only Qt5 is available, and 54 | is backwards compatible with Qt4 for Indigo. 55 | * Contributors: Martin Guenther 56 | 57 | 1.0.9 (2015-10-16) 58 | ------------------ 59 | 60 | 1.0.8 (2015-10-07) 61 | ------------------ 62 | 63 | 1.0.7 (2015-10-07) 64 | ------------------ 65 | 66 | 1.0.6 (2015-10-06) 67 | ------------------ 68 | 69 | 1.0.5 (2015-06-24) 70 | ------------------ 71 | 72 | 1.0.4 (2015-05-06) 73 | ------------------ 74 | 75 | 1.0.3 (2015-01-29) 76 | ------------------ 77 | 78 | 1.0.2 (2015-01-27) 79 | ------------------ 80 | 81 | 1.0.1 (2014-12-10) 82 | ------------------ 83 | * add me as maintainer to package.xml 84 | * Contributors: Martin Günther 85 | 86 | 1.0.0 (2014-09-03) 87 | ------------------ 88 | * First public release 89 | * Contributors: Ivan Dryanovski, Martin Günther, Davide Tateo, Francisco Vina, Lorenzo Riano 90 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package imu_complementary_filter 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.8 (2020-05-25) 6 | ------------------ 7 | * fix install path & boost linkage issues 8 | * Contributors: Sean Yen 9 | 10 | 1.1.7 (2019-05-06) 11 | ------------------ 12 | * Remove junk xml (`#93 `_) 13 | * Fix C++14 builds (`#89 `_) 14 | * Contributors: David V. Lu!!, Paul Bovbel 15 | 16 | 1.1.6 (2018-05-25) 17 | ------------------ 18 | * Add std dev parameter to orientation estimate from filter (`#85 `_) 19 | Similar to `#41 `_, but not using dynamic_reconfigure as not implemented for complementary filter 20 | * Contributors: Stefan Kohlbrecher 21 | 22 | 1.1.4 (2017-05-22) 23 | ------------------ 24 | 25 | 1.1.3 (2017-03-10) 26 | ------------------ 27 | * complementary_filter: move const initializations out of header 28 | Initialization of static consts other than int (here: float) inside the 29 | class declaration is not permitted in C++. It works in gcc (due to a 30 | non-standard extension), but throws an error in C++11. 31 | * Contributors: Martin Guenther 32 | 33 | 1.1.2 (2016-09-07) 34 | ------------------ 35 | 36 | 1.1.1 (2016-09-07) 37 | ------------------ 38 | 39 | 1.1.0 (2016-04-25) 40 | ------------------ 41 | 42 | 1.0.11 (2016-04-22) 43 | ------------------- 44 | 45 | 1.0.10 (2016-04-22) 46 | ------------------- 47 | * Remove Eigen dependency 48 | Eigen is not actually used anywhere. Thanks @asimay! 49 | * Removed main function from shared library 50 | * Contributors: Martin Guenther, Matthias Nieuwenhuisen 51 | 52 | 1.0.9 (2015-10-16) 53 | ------------------ 54 | * complementary: Add Eigen dependency 55 | Fixes `#54 `_. 56 | * Contributors: Martin Günther 57 | 58 | 1.0.8 (2015-10-07) 59 | ------------------ 60 | 61 | 1.0.7 (2015-10-07) 62 | ------------------ 63 | * Allow remapping imu namespace 64 | * Publish RPY as Vector3Stamped 65 | * Add params: constant_dt, publish_tf, reverse_tf, publish_debug_topics 66 | * Use MagneticField instead of Vector3 67 | * Contributors: Martin Günther 68 | 69 | 1.0.6 (2015-10-06) 70 | ------------------ 71 | * Add new package: imu_complementary_filter 72 | * Contributors: Roberto G. Valentini, Martin Günther, Michael Görner 73 | 74 | 1.0.5 (2015-06-24) 75 | ------------------ 76 | 77 | 1.0.4 (2015-05-06) 78 | ------------------ 79 | 80 | 1.0.3 (2015-01-29) 81 | ------------------ 82 | 83 | 1.0.2 (2015-01-27) 84 | ------------------ 85 | 86 | 1.0.1 (2014-12-10) 87 | ------------------ 88 | 89 | 1.0.0 (2014-11-28) 90 | ------------------ 91 | -------------------------------------------------------------------------------- /racecar/launch/amcl_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /racecar_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | racecar_driver 4 | 0.1.0 5 | The racecar_driver package 6 | 7 | 8 | 9 | 10 | *** 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 | catkin 51 | roscpp 52 | rospy 53 | std_msgs 54 | roscpp 55 | rospy 56 | std_msgs 57 | roscpp 58 | rospy 59 | std_msgs 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /racecar/launch/amcl_nav copy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /encoder_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | encoder_driver 4 | 0.0.0 5 | The encoder_driver package 6 | 7 | 8 | 9 | 10 | hzx 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 | serial 53 | serial 54 | geometry_msgs 55 | geometry_msgs 56 | tf 57 | tf 58 | roscpp 59 | rospy 60 | std_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | roscpp 65 | rospy 66 | std_msgs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /serial_imu/src/imu_data_decode.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "packet.h" 5 | #include "imu_data_decode.h" 6 | 7 | static packet_t RxPkt; /* used for data receive */ 8 | /* 9 | **采用结构体来保存数据 10 | **将标志位都集中到一个32位的变量上，用位来表示 11 | **在复制数据时，在用户程序中直接调用一个memcpy函数 12 | */ 13 | 14 | uint8_t bitmap; 15 | id0x91_t id0x91; /* HI226 HI229 CH100 CH110 HI221 protocol packet */ 16 | id0x62_t id0x62; /* HI221 Dongle protocol packet */ 17 | 18 | static int stream2int16(int *dest,uint8_t *src) 19 | { 20 | dest[0] = (int16_t)(src[0] | src[1] << 8); 21 | dest[1] = (int16_t)(src[2] | src[3] << 8); 22 | dest[2] = (int16_t)(src[4] | src[5] << 8); 23 | return 0; 24 | } 25 | 26 | 27 | /* callback function of when recv a data frame successfully */ 28 | static void on_data_received(packet_t *pkt) 29 | { 30 | int temp[3] = {0}; 31 | int offset = 0; 32 | uint8_t *p = pkt->buf; 33 | 34 | if(pkt->type != 0xA5) 35 | { 36 | return; 37 | } 38 | 39 | bitmap = 0; 40 | while(offset < pkt->payload_len) 41 | { 42 | switch(p[offset]) 43 | { 44 | case kItemID: 45 | bitmap |= BIT_VALID_ID; 46 | id0x91.id = p[1]; 47 | offset += 2; 48 | break; 49 | 50 | case kItemAccRaw: 51 | bitmap |= BIT_VALID_ACC; 52 | stream2int16(temp, p + offset + 1); 53 | id0x91.acc[0] = (float)temp[0] / 1000; 54 | id0x91.acc[1] = (float)temp[1] / 1000; 55 | id0x91.acc[2] = (float)temp[2] / 1000; 56 | offset += 7; 57 | break; 58 | 59 | case kItemGyrRaw: 60 | case kItemGyrRaw_yunjing: 61 | bitmap |= BIT_VALID_GYR; 62 | stream2int16(temp, p + offset + 1); 63 | id0x91.gyr[0] = (float)temp[0] / 10; 64 | id0x91.gyr[1] = (float)temp[1] / 10; 65 | id0x91.gyr[2] = (float)temp[2] / 10; 66 | offset += 7; 67 | break; 68 | 69 | case kItemMagRaw: 70 | bitmap |= BIT_VALID_MAG; 71 | stream2int16(temp, p + offset + 1); 72 | id0x91.mag[0] = (float)temp[0] / 10; 73 | id0x91.mag[1] = (float)temp[1] / 10; 74 | id0x91.mag[2] = (float)temp[2] / 10; 75 | offset += 7; 76 | break; 77 | 78 | case kItemRotationEul: 79 | bitmap |= BIT_VALID_EUL; 80 | stream2int16(temp, p + offset + 1); 81 | id0x91.eul[1] = (float)temp[0] / 100; 82 | id0x91.eul[0] = (float)temp[1] / 100; 83 | id0x91.eul[2] = (float)temp[2] / 10; 84 | offset += 7; 85 | break; 86 | 87 | case kItemRotationQuat: 88 | bitmap |= BIT_VALID_QUAT; 89 | memcpy(id0x91.quat, p + offset + 1, sizeof( id0x91.quat)); 90 | offset += 17; 91 | break; 92 | 93 | case kItemPressure: 94 | offset += 5; 95 | break; 96 | 97 | case KItemIMUSOL: 98 | bitmap = BIT_VALID_ALL; 99 | memcpy((void *)&id0x91, p, sizeof(id0x91_t)); 100 | offset += sizeof(id0x91_t); 101 | break; 102 | 103 | case KItemGWSOL: 104 | memcpy((void *)&id0x62, p, 8); 105 | offset += 8; 106 | for (int i = 0; i < id0x62.n; i++) 107 | { 108 | bitmap = BIT_VALID_ALL; 109 | bitmap &= ~BIT_VALID_TIME; 110 | memcpy((void *)&id0x62.id0x91[i], p + offset, sizeof(id0x91_t)); 111 | 112 | offset += sizeof(id0x91_t); 113 | } 114 | break; 115 | 116 | default: 117 | /* offset ==> 0 2 9 16 23 30 47 52 76 */ 118 | offset++; 119 | } 120 | } 121 | } 122 | 123 | 124 | int imu_data_decode_init(void) 125 | { 126 | packet_decode_init(&RxPkt, on_data_received); 127 | return 0; 128 | } 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_IMU_FILTER_H 26 | #define IMU_FILTER_MADWICK_IMU_FILTER_H 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | class ImuFilter 33 | { 34 | public: 35 | 36 | ImuFilter(); 37 | virtual ~ImuFilter(); 38 | 39 | private: 40 | // **** paramaters 41 | double gain_; // algorithm gain 42 | double zeta_; // gyro drift bias gain 43 | WorldFrame::WorldFrame world_frame_; // NWU, ENU, NED 44 | 45 | // **** state variables 46 | double q0, q1, q2, q3; // quaternion 47 | float w_bx_, w_by_, w_bz_; // 48 | 49 | public: 50 | void setAlgorithmGain(double gain) 51 | { 52 | gain_ = gain; 53 | } 54 | 55 | void setDriftBiasGain(double zeta) 56 | { 57 | zeta_ = zeta; 58 | } 59 | 60 | void setWorldFrame(WorldFrame::WorldFrame frame) 61 | { 62 | world_frame_ = frame; 63 | } 64 | 65 | void getOrientation(double& q0, double& q1, double& q2, double& q3) 66 | { 67 | q0 = this->q0; 68 | q1 = this->q1; 69 | q2 = this->q2; 70 | q3 = this->q3; 71 | 72 | // perform precise normalization of the output, using 1/sqrt() 73 | // instead of the fast invSqrt() approximation. Without this, 74 | // TF2 complains that the quaternion is not normalized. 75 | double recipNorm = 1 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 76 | q0 *= recipNorm; 77 | q1 *= recipNorm; 78 | q2 *= recipNorm; 79 | q3 *= recipNorm; 80 | } 81 | 82 | void setOrientation(double q0, double q1, double q2, double q3) 83 | { 84 | this->q0 = q0; 85 | this->q1 = q1; 86 | this->q2 = q2; 87 | this->q3 = q3; 88 | 89 | w_bx_ = 0; 90 | w_by_ = 0; 91 | w_bz_ = 0; 92 | } 93 | 94 | void madgwickAHRSupdate(float gx, float gy, float gz, 95 | float ax, float ay, float az, 96 | float mx, float my, float mz, 97 | float dt); 98 | 99 | void madgwickAHRSupdateIMU(float gx, float gy, float gz, 100 | float ax, float ay, float az, 101 | float dt); 102 | 103 | void getGravity(float& rx, float& ry, float& rz, 104 | float gravity = 9.80665); 105 | }; 106 | 107 | #endif // IMU_FILTER_IMU_MADWICK_FILTER_H 108 | -------------------------------------------------------------------------------- /racecar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | racecar 4 | 0.1.0 5 | The racecar package 6 | 7 | 8 | 9 | 10 | *** 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 | joy 54 | move_base 55 | roscpp 56 | rospy 57 | std_msgs 58 | visualization_msgs 59 | tf 60 | sensor_msgs 61 | python-serial 62 | dynamic_reconfigure 63 | 64 | 65 | geometry_msgs 66 | move_base 67 | joy 68 | roscpp 69 | rospy 70 | std_msgs 71 | visualization_msgs 72 | tf 73 | tf 74 | sensor_msgs 75 | python-serial 76 | dynamic_reconfigure 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /serial_imu/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | serial_imu 4 | 0.0.0 5 | The serial_imu package 6 | 7 | 8 | 9 | 10 | linux 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 | roscpp 54 | rospy 55 | serial 56 | std_msgs 57 | turtlesim 58 | geometry_msgs 59 | roscpp 60 | rospy 61 | serial 62 | std_msgs 63 | turtlesim 64 | geometry_msgs 65 | roscpp 66 | rospy 67 | serial 68 | std_msgs 69 | turtlesim 70 | 71 | http://ros.org/wiki/sensor_mags 72 | 73 | 74 | message_generation 75 | message_runtime 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /encoder_driver/src/Encoder_vel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | #coding:UTF-8 3 | 4 | import rospy 5 | from rospy.timer import Rate 6 | import serial 7 | import time 8 | 9 | from nav_msgs.msg import Odometry 10 | 11 | 12 | 13 | Bytenum_vel = 0 #读取到这一段的第几位 14 | Bytenum_dir = 0 15 | last_pose = 0 16 | C = 227.45/1000 17 | 18 | 19 | def DueVelData(inputdata): #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里 20 | #在局部修改全局变量，要进行global的定义 21 | global Bytenum_vel 22 | 23 | for data in inputdata: #在输入的数据进行遍历 'ex:01 03 04 02 7A (D8 C6)' 24 | # print(data) 25 | if data==0x01 and Bytenum_vel==0: 26 | Bytenum_vel = 1 27 | continue 28 | if data==0x03 and Bytenum_vel==1: 29 | Bytenum_vel = 2 30 | continue 31 | if data==0x02 and Bytenum_vel==2: 32 | Bytenum_vel = 3 33 | continue 34 | if Bytenum_vel==3: 35 | data_high = data 36 | Bytenum_vel = 4 37 | continue 38 | if Bytenum_vel==4: 39 | data_low = data 40 | Bytenum_vel = 0 41 | Angle_vel= data_high * 256 + data_low 42 | return float(Angle_vel) 43 | 44 | def DueDirData(inputdata): #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里 45 | #在局部修改全局变量，要进行global的定义 46 | global Bytenum_dir 47 | global last_pose 48 | 49 | for data in inputdata: #在输入的数据进行遍历 'ex:01 03 02 01 42 (39 E5)' 50 | # print(data) 51 | if data==0x01 and Bytenum_dir==0: 52 | Bytenum_dir = 1 53 | continue 54 | if data==0x03 and Bytenum_dir==1: 55 | Bytenum_dir = 2 56 | continue 57 | if data==0x02 and Bytenum_dir==2: 58 | Bytenum_dir = 3 59 | continue 60 | if Bytenum_dir==3: 61 | data_high = data 62 | Bytenum_dir = 4 63 | continue 64 | if Bytenum_dir==4: 65 | data_low = data 66 | Bytenum_dir = 0 67 | position = data_high * 256 + data_low 68 | pose = position - last_pose 69 | last_pose = position 70 | 71 | if (pose>=0 and pose<512) or (pose> -1024 and pose< -512): 72 | direction = 1 73 | elif (pose<0 and pose> -512) or (pose < 1024 and pose > 512): 74 | direction = -1 75 | 76 | return int(direction) 77 | 78 | def init_topic(): 79 | 80 | 81 | return 0 82 | 83 | 84 | if __name__=='__main__': 85 | rospy.init_node('encoder_vel', log_level=rospy.DEBUG) 86 | pub = rospy.Publisher('encoder', Odometry, queue_size=10) 87 | 88 | port = rospy.get_param('~serial_port', '/dev/ttyUSB0') 89 | baud = rospy.get_param('~baud_rate', 57600) # about 100hz 90 | k = rospy.get_param('~k',1) # fix param 91 | 92 | ser = serial.Serial(port, baud) 93 | print(ser.is_open) 94 | 95 | while( not rospy.is_shutdown()): 96 | # time1 = time.time() 97 | send_data = bytes.fromhex('01 03 00 03 00 01 74 0A') # read velocity value in 20ms 98 | ser.write(send_data) 99 | datahex = ser.read(7) 100 | angle_v = DueVelData(datahex) 101 | 102 | send_data = bytes.fromhex('01 03 00 00 00 01 84 0A') # 103 | ser.write(send_data) 104 | datahex = ser.read(7) 105 | dir = DueDirData(datahex) 106 | 107 | Vel = angle_v * dir * C / 1024.0 / 0.02 * k 108 | # print(Vel) 109 | # time2 = time.time() 110 | # print(1/(time2-time1)) 111 | 112 | pub_vel = Odometry() 113 | pub_vel.header.frame_id = 'odom' 114 | pub_vel.child_frame_id = 'base_footprint' 115 | pub_vel.header.stamp = rospy.Time.now() 116 | pub_vel.twist.twist.linear.x = Vel 117 | pub.publish(pub_vel) 118 | 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_axes_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 32 | #define RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace rviz 40 | { 41 | 42 | class Axes; 43 | 44 | class ImuAxesVisual 45 | { 46 | public: 47 | // Constructor. Creates the visual stuff and puts it into the 48 | // scene, but in an unconfigured state. 49 | ImuAxesVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node); 50 | 51 | // Destructor. Removes the visual stuff from the scene. 52 | virtual ~ImuAxesVisual(); 53 | 54 | // Configure the visual to show the data in the message. 55 | void setMessage(const sensor_msgs::Imu::ConstPtr& msg); 56 | 57 | // Set the pose of the coordinate frame the message refers to. 58 | // These could be done inside setMessage(), but that would require 59 | // calls to FrameManager and error handling inside setMessage(), 60 | // which doesn't seem as clean. This way ImuVisual is only 61 | // responsible for visualization. 62 | void setFramePosition(const Ogre::Vector3& position); 63 | void setFrameOrientation(const Ogre::Quaternion& orientation); 64 | 65 | // Set the color and alpha of the visual, which are user-editable 66 | 67 | void setScale(float scale); 68 | 69 | float getScale() { return scale_; } 70 | 71 | void show(); 72 | void hide(); 73 | 74 | private: 75 | 76 | void create(); 77 | inline bool checkQuaternionValidity( 78 | const sensor_msgs::Imu::ConstPtr& msg); 79 | 80 | Ogre::Quaternion orientation_; 81 | 82 | float scale_; 83 | bool quat_valid_; 84 | 85 | Axes * orientation_axes_; 86 | 87 | // A SceneNode whose pose is set to match the coordinate frame of 88 | // the Imu message header. 89 | Ogre::SceneNode * frame_node_; 90 | 91 | // The SceneManager, kept here only so the destructor can ask it to 92 | // destroy the ``frame_node_``. 93 | Ogre::SceneManager * scene_manager_; 94 | }; 95 | 96 | } // end namespace rviz 97 | 98 | #endif // RVIZ_IMU_PLUGIN_IMU_AXES_VISUAL_H 99 | -------------------------------------------------------------------------------- /racecar/src/racecar_joy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | from sensor_msgs.msg import Joy 7 | 8 | import sys, select, termios, tty 9 | import time 10 | 11 | msg = """ 12 | 13 | CTRL-C to quit 14 | """ 15 | global run 16 | global speed_add_once,turn_add_once,speed_mid,turn_mid,control_speed,control_turn,speed_max,turn_max,speed_min,turn_min 17 | global last_turn_add_once 18 | 19 | 20 | 21 | def callback(data): 22 | global last_speed_add_once 23 | twist = Twist() 24 | car_twist = Twist() 25 | global speed_add_once,turn_add_once 26 | global last_turn_add_once,last_speed_add_once 27 | if (data.buttons[6] == 1 or data.buttons[7] == 1): 28 | twist.linear.x = 0 29 | twist.angular.z = 0 30 | print("STOP!!") 31 | else: 32 | if(data.axes[5] != 0): 33 | if(last_speed_add_once == 0): 34 | speed_add_once = speed_add_once + data.axes[5] * 0.1 35 | last_speed_add_once = 1 36 | else: 37 | last_speed_add_once = 0 38 | if(data.axes[4] != 0): 39 | if(last_turn_add_once == 0): 40 | turn_add_once = turn_add_once + data.axes[4] * 0.1 41 | last_turn_add_once = 1 42 | else: 43 | last_turn_add_once = 0 44 | if(speed_add_once > 1): 45 | speed_add_once = 1 46 | elif(speed_add_once < -1): 47 | speed_add_once = -1 48 | if(turn_add_once > 1): 49 | turn_add_once = 1 50 | elif(turn_add_once < -1): 51 | turn_add_once = -1 52 | twist.linear.x = data.axes[1] * speed_add_once 53 | twist.angular.z = data.axes[3] * turn_add_once 54 | control_speed = (data.axes[1] * (speed_max - speed_min) * speed_add_once / 2 + speed_mid) 55 | if(control_speed > 1500): 56 | control_speed = control_speed + 350 57 | elif(control_speed < 1500): 58 | control_speed = control_speed - 50 59 | control_turn = (data.axes[3] * (turn_max - turn_min) * turn_add_once / 2 + turn_mid) 60 | # print('speed: %.2f, turn: %.2f'%(twist.linear.x,twist.angular.z)) 61 | car_twist.linear.x = control_speed 62 | car_twist.angular.z = control_turn 63 | print('speed: %.2f, turn: %.2f'%(control_speed,control_turn)) 64 | pub.publish(twist) 65 | pub_car.publish(car_twist) 66 | 67 | 68 | 69 | def vels(speed,turn): 70 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 71 | 72 | 73 | def getKey(): 74 | tty.setraw(sys.stdin.fileno()) 75 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 76 | if rlist: 77 | key = sys.stdin.read(1) 78 | else: 79 | key = '' 80 | 81 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 82 | return key 83 | 84 | 85 | if __name__=="__main__": 86 | 87 | settings = termios.tcgetattr(sys.stdin) 88 | speed_add_once = 0.25 89 | turn_add_once = 0.5 90 | speed_max = 1900 91 | speed_min = 1400 92 | speed_mid = 1500 93 | turn_max = 180 94 | turn_min = 0 95 | turn_mid = (turn_max + turn_min)/2 96 | 97 | control_speed = speed_mid 98 | control_turn = turn_mid 99 | 100 | global pub 101 | rospy.init_node('racecar_joy') 102 | pub = rospy.Publisher('~/cmd_vel', Twist, queue_size=5) 103 | pub_car = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5) 104 | try: 105 | 106 | while(1): 107 | 108 | key = getKey() 109 | 110 | rospy.Subscriber("joy", Joy, callback) 111 | 112 | rospy.spin() 113 | 114 | if (key == '\x03'): #for ctrl + c exit 115 | break 116 | 117 | 118 | except: 119 | print("error") 120 | 121 | finally: 122 | print("finally") 123 | twist = Twist() 124 | car_twist = Twist() 125 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 126 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 127 | pub.publish(twist) 128 | pub_car.publish(car_twist) 129 | 130 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 131 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_acc_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #ifndef RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H 31 | #define RVIZ_IMU_PLUGIN_IMU_ACC_VISUAL_H 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | namespace rviz 42 | { 43 | 44 | class ImuAccVisual 45 | { 46 | public: 47 | // Constructor. Creates the visual stuff and puts it into the 48 | // scene, but in an unconfigured state. 49 | ImuAccVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node); 50 | 51 | // Destructor. Removes the visual stuff from the scene. 52 | virtual ~ImuAccVisual(); 53 | 54 | // Configure the visual to show the data in the message. 55 | void setMessage(const sensor_msgs::Imu::ConstPtr& msg); 56 | 57 | // Set the pose of the coordinate frame the message refers to. 58 | // These could be done inside setMessage(), but that would require 59 | // calls to FrameManager and error handling inside setMessage(), 60 | // which doesn't seem as clean. This way ImuVisual is only 61 | // responsible for visualization. 62 | void setFramePosition(const Ogre::Vector3& position); 63 | void setFrameOrientation(const Ogre::Quaternion& orientation); 64 | 65 | // Set the color and alpha of the visual, which are user-editable 66 | 67 | void setScale(float scale); 68 | void setColor(const QColor &color); 69 | void setAlpha(float alpha); 70 | void setDerotated(bool derotated); 71 | 72 | float getScale() { return scale_; } 73 | const QColor& getColor() { return color_; } 74 | float getAlpha() { return alpha_; } 75 | bool getDerotated() { return derotated_; } 76 | 77 | void show(); 78 | void hide(); 79 | 80 | private: 81 | 82 | void create(); 83 | 84 | Arrow * acc_vector_; 85 | 86 | Ogre::Vector3 direction_; // computed from IMU message 87 | 88 | float arrow_length_; // computed from IMU message 89 | float arrow_radius_; 90 | float head_length_; 91 | float head_radius_; 92 | 93 | float scale_; 94 | float alpha_; 95 | QColor color_; 96 | 97 | bool derotated_; 98 | 99 | // A SceneNode whose pose is set to match the coordinate frame of 100 | // the Imu message header. 101 | Ogre::SceneNode * frame_node_; 102 | 103 | // The SceneManager, kept here only so the destructor can ask it to 104 | // destroy the ``frame_node_``. 105 | Ogre::SceneManager * scene_manager_; 106 | }; 107 | 108 | } // end namespace rviz 109 | 110 | #endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 111 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_orientation_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #ifndef RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 31 | #define RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace rviz 41 | { 42 | 43 | class ImuOrientationVisual 44 | { 45 | public: 46 | // Constructor. Creates the visual stuff and puts it into the 47 | // scene, but in an unconfigured state. 48 | ImuOrientationVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node); 49 | 50 | // Destructor. Removes the visual stuff from the scene. 51 | virtual ~ImuOrientationVisual(); 52 | 53 | // Configure the visual to show the data in the message. 54 | void setMessage(const sensor_msgs::Imu::ConstPtr& msg); 55 | 56 | // Set the pose of the coordinate frame the message refers to. 57 | // These could be done inside setMessage(), but that would require 58 | // calls to FrameManager and error handling inside setMessage(), 59 | // which doesn't seem as clean. This way ImuVisual is only 60 | // responsible for visualization. 61 | void setFramePosition(const Ogre::Vector3& position); 62 | void setFrameOrientation(const Ogre::Quaternion& orientation); 63 | 64 | // Set the color and alpha of the visual, which are user-editable 65 | 66 | void setScaleX(float x); 67 | void setScaleY(float y); 68 | void setScaleZ(float z); 69 | void setColor(const QColor &color); 70 | void setAlpha(float alpha); 71 | 72 | float getScaleX() { return scale_x_; } 73 | float getScaleY() { return scale_y_; } 74 | float getScaleZ() { return scale_z_; } 75 | const QColor& getColor() { return color_; } 76 | float getAlpha() { return alpha_; } 77 | 78 | void show(); 79 | void hide(); 80 | 81 | private: 82 | 83 | void create(); 84 | inline bool checkQuaternionValidity( 85 | const sensor_msgs::Imu::ConstPtr& msg); 86 | 87 | Ogre::Quaternion orientation_; 88 | 89 | float scale_x_, scale_y_, scale_z_; 90 | QColor color_; 91 | float alpha_; 92 | bool quat_valid_; 93 | 94 | Shape * orientation_box_; 95 | 96 | // A SceneNode whose pose is set to match the coordinate frame of 97 | // the Imu message header. 98 | Ogre::SceneNode * frame_node_; 99 | 100 | // The SceneManager, kept here only so the destructor can ask it to 101 | // destroy the ``frame_node_``. 102 | Ogre::SceneManager * scene_manager_; 103 | }; 104 | 105 | } // end namespace rviz 106 | 107 | #endif // RVIZ_IMU_PLUGIN_IMU_ORIENTATATION_VISUAL_H 108 | -------------------------------------------------------------------------------- /imu_tool/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | @author Roberto G. Valenti 3 | 4 | @section LICENSE 5 | Copyright (c) 2015, City University of New York 6 | CCNY Robotics Lab 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 1. Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | 2. Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 3. Neither the name of the City College of New York nor the 17 | names of its contributors may be used to endorse or promote products 18 | derived from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 33 | #define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include "imu_complementary_filter/complementary_filter.h" 46 | 47 | namespace imu_tools { 48 | 49 | class ComplementaryFilterROS 50 | { 51 | public: 52 | ComplementaryFilterROS(const ros::NodeHandle& nh, 53 | const ros::NodeHandle& nh_private); 54 | virtual ~ComplementaryFilterROS(); 55 | 56 | private: 57 | 58 | // Convenience typedefs 59 | typedef sensor_msgs::Imu ImuMsg; 60 | typedef sensor_msgs::MagneticField MagMsg; 61 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 63 | typedef message_filters::sync_policies::ApproximateTime 64 | SyncPolicy; 65 | typedef message_filters::Synchronizer Synchronizer; 66 | typedef message_filters::Subscriber ImuSubscriber; 67 | typedef message_filters::Subscriber MagSubscriber; 68 | 69 | // ROS-related variables. 70 | ros::NodeHandle nh_; 71 | ros::NodeHandle nh_private_; 72 | 73 | boost::shared_ptr sync_; 74 | boost::shared_ptr imu_subscriber_; 75 | boost::shared_ptr mag_subscriber_; 76 | 77 | ros::Publisher imu_publisher_; 78 | ros::Publisher rpy_publisher_; 79 | ros::Publisher state_publisher_; 80 | tf::TransformBroadcaster tf_broadcaster_; 81 | 82 | // Parameters: 83 | bool use_mag_; 84 | bool publish_tf_; 85 | bool reverse_tf_; 86 | double constant_dt_; 87 | bool publish_debug_topics_; 88 | std::string fixed_frame_; 89 | double orientation_variance_; 90 | 91 | // State variables: 92 | ComplementaryFilter filter_; 93 | ros::Time time_prev_; 94 | bool initialized_filter_; 95 | 96 | void initializeParams(); 97 | void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw); 98 | void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, 99 | const MagMsg::ConstPtr& mav_msg); 100 | void publish(const sensor_msgs::Imu::ConstPtr& imu_msg_raw); 101 | 102 | tf::Quaternion hamiltonToTFQuaternion( 103 | double q0, double q1, double q2, double q3) const; 104 | }; 105 | 106 | } // namespace imu_tools 107 | 108 | #endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 109 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/test/test_helpers.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef TEST_TEST_HELPERS_H_ 3 | #define TEST_TEST_HELPERS_H_ 4 | 5 | #include 6 | #include 7 | 8 | #define MAX_DIFF 0.05 9 | 10 | template 11 | static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) { 12 | T invNorm = 1 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); 13 | T max = q0; 14 | if (fabs(max) < fabs(q1)) max = q1; 15 | if (fabs(max) < fabs(q2)) max = q2; 16 | if (fabs(max) < fabs(q3)) max = q3; 17 | if (max < 0) invNorm *= -1.0; 18 | 19 | q0 *= invNorm; 20 | q1 *= invNorm; 21 | q2 *= invNorm; 22 | q3 *= invNorm; 23 | } 24 | 25 | // Tests for normalization in the same way as tf2: 26 | // https://github.com/ros/geometry2/blob/bd490515b1434caeff521ea14901dfe04063ca27/tf2/src/buffer_core.cpp#L244-L247 27 | template 28 | static inline bool is_normalized(T q0, T q1, T q2, T q3) { 29 | return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6; 30 | } 31 | 32 | template 33 | static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) { 34 | normalize_quaternion(q0, q1, q2, q3); 35 | normalize_quaternion(qr0, qr1, qr2, qr3); 36 | 37 | return (fabs(q0 - qr0) < MAX_DIFF) && 38 | (fabs(q1 - qr1) < MAX_DIFF) && 39 | (fabs(q2 - qr2) < MAX_DIFF) && 40 | (fabs(q3 - qr3) < MAX_DIFF); 41 | } 42 | 43 | template 44 | static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) { 45 | // assert q == qr * qz 46 | tf2::Quaternion q(q1, q2, q3, q0); 47 | tf2::Quaternion qr(qr1, qr2, qr3, qr0); 48 | tf2::Quaternion qz = q * qr.inverse(); 49 | 50 | // remove x and y components. 51 | qz.setX(0.0); 52 | qz.setY(0.0); 53 | 54 | tf2::Quaternion qr_ = qz * qr; 55 | 56 | return quat_equal(q0, q1, q2, q3, 57 | qr_.getW(), 58 | qr_.getX(), 59 | qr_.getY(), 60 | qr_.getZ()); 61 | } 62 | 63 | #define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 64 | #define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__) 65 | 66 | #define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 67 | #define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__) 68 | 69 | #define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 70 | #define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__) 71 | 72 | // Well known states 73 | // scheme: AM_x_y_z 74 | #define AM_EAST_NORTH_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ 0.0, 0.0005, -0.0005 75 | #define AM_NORTH_EAST_DOWN /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0005, 0.0, 0.0005 76 | #define AM_NORTH_WEST_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ 0.0005, 0.0, -0.0005 77 | #define AM_SOUTH_UP_WEST /* Acceleration */ 0.0, 9.81, 0.0, /* Magnetic */ -0.0005, -0.0005, 0.0 78 | #define AM_SOUTH_EAST_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ -0.0005, 0.0, -0.0005 79 | #define AM_WEST_NORTH_DOWN /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0, 0.0005, 0.0005 80 | 81 | // Real sensor data 82 | #define AM_WEST_NORTH_DOWN_RSD /* Acceleration */ 0.12, 0.29, -9.83, /* Magnetic */ 6.363e-06, 1.0908e-05, 4.2723e-05 83 | #define AM_NE_NW_UP_RSD /* Acceleration */ 0.20, 0.55, 9.779, /* Magnetic */ 8.484e-06, 8.181e-06, -4.3329e-05 84 | 85 | // Strip accelration from am 86 | #define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az 87 | 88 | // Well known quaternion 89 | // scheme: QUAT_axis_angle 90 | #define QUAT_IDENTITY 1.0, 0.0, 0.0, 0.0 91 | #define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107 92 | #define QUAT_X_180 0.0, 1.0, 0.0, 0.0 93 | #define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5 94 | #define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 /* axis: (0.864401, 0.502559, 0.0120614) | angle: 178.848deg */ 95 | #define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02/* Axis: (-0.2, -0.96, 0.02), Angle: 180deg */ 96 | #define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 /* Axis: -Z, Angle: 60deg */ 97 | #define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 /* axis: (0.0300376, -0.020025, -0.410513) | angle: 48.6734deg */ 98 | #define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 /* Axis: Z, Angle: 41deg */ 99 | #define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02 /* Axis: (0.9, 0.4, 0.0), Angle: 180deg */ 100 | 101 | 102 | #endif /* TEST_TEST_HELPERS_H_ */ 103 | -------------------------------------------------------------------------------- /serial_imu/src/sub_0x62.cpp: -------------------------------------------------------------------------------- 1 | //subscriber 0x62 data package 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "imu_data_decode.h" 8 | 9 | void imu_0x62_callback(const serial_imu::Imu_0x62_msg imu_0x62_msg); 10 | 11 | int main(int argc, char **argv) 12 | { 13 | 14 | ros::init(argc, argv, "sub_0x62"); 15 | 16 | ros::NodeHandle n; 17 | 18 | ros::Subscriber imu_0x62_sub = n.subscribe("/imu_0x62_package", 10,imu_0x62_callback); 19 | 20 | ros::spin(); 21 | 22 | } 23 | 24 | void imu_0x62_callback(const serial_imu::Imu_0x62_msg imu_0x62_msg) 25 | { 26 | int i = 0; 27 | 28 | 29 | printf("\033c"); 30 | 31 | printf(" Device GWID: %6d\n", imu_0x62_msg.gw_id); 32 | printf(" Device number:%6d\n", imu_0x62_msg.node_num); 33 | if(imu_0x62_msg.node_num == 1) 34 | { 35 | i = 2; 36 | goto ONE_NODE; 37 | } 38 | 39 | for (i = 0; i < imu_0x62_msg.node_num; i += 2) 40 | { 41 | putchar(10); 42 | #if 0 43 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_ID) 44 | printf(" Devie ID:%6d\n",imu_0x62_msg.node_data[i].id); 45 | 46 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_PRS) 47 | printf(" Prs(hPa): %6f\n", imu_0x62_msg.node_data[i].prs); 48 | 49 | 50 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_TIME) 51 | printf(" Run times: %d days %d:%d:%d:%d\n",imu_0x62_msg.node_data[i].time / 86400000, imu_0x62_msg.node_data[i].time / 3600000 % 24, imu_0x62_msg.node_data[i].time / 60000 % 60, imu_0x62_msg.node_data[i].time / 1000 % 60, imu_0x62_msg.node_data[i].time % 1000); 52 | 53 | printf(" Frame Rate: %4dHz\r\n", imu_0x62_msg.node_data[i].frame_rate); 54 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_ACC) 55 | printf(" Acc(G):%8.3f %8.3f %8.3f\r\n", imu_0x62_msg.node_data[i].acc_x, imu_0x62_msg.node_data[i].acc_y, imu_0x62_msg.node_data[i].acc_z); 56 | 57 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_GYR) 58 | printf(" Gyr(deg/s):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].gyr_x, imu_0x62_msg.node_data[i].gyr_y, imu_0x62_msg.node_data[i].gyr_z); 59 | 60 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_MAG) 61 | printf(" Mag(uT):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].mag_x, imu_0x62_msg.node_data[i].mag_y, imu_0x62_msg.node_data[i].mag_z); 62 | 63 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_EUL) 64 | printf(" Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].eul_r, imu_0x62_msg.node_data[i].eul_p, imu_0x62_msg.node_data[i].eul_y); 65 | 66 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_QUAT) 67 | printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu_0x62_msg.node_data[i].quat_w, imu_0x62_msg.node_data[i].quat_x, imu_0x62_msg.node_data[i].quat_y, imu_0x62_msg.node_data[i].quat_z); 68 | #endif 69 | } 70 | 71 | ONE_NODE: 72 | if(i - 1 == imu_0x62_msg.node_num) 73 | { 74 | putchar(10); 75 | i -= 2; 76 | #if 1 77 | 78 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_ID) 79 | printf(" Devie ID:%6d\n",imu_0x62_msg.node_data[i].id); 80 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_PRS) 81 | printf(" Prs(hPa): %6f\n", imu_0x62_msg.node_data[i].prs); 82 | 83 | 84 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_TIME) 85 | printf(" Run times: %d days %d:%d:%d:%d\n",imu_0x62_msg.node_data[i].time / 86400000, imu_0x62_msg.node_data[i].time / 3600000 % 24, imu_0x62_msg.node_data[i].time / 60000 % 60, imu_0x62_msg.node_data[i].time / 1000 % 60, imu_0x62_msg.node_data[i].time % 1000); 86 | 87 | printf(" Frame Rate: %4dHz\r\n", imu_0x62_msg.node_data[i].frame_rate); 88 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_ACC) 89 | printf(" Acc(G):%8.3f %8.3f %8.3f\r\n", imu_0x62_msg.node_data[i].acc_x, imu_0x62_msg.node_data[i].acc_y, imu_0x62_msg.node_data[i].acc_z); 90 | 91 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_GYR) 92 | printf(" Gyr(deg/s):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].gyr_x, imu_0x62_msg.node_data[i].gyr_y, imu_0x62_msg.node_data[i].gyr_z); 93 | 94 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_MAG) 95 | printf(" Mag(uT):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].mag_x, imu_0x62_msg.node_data[i].mag_y, imu_0x62_msg.node_data[i].mag_z); 96 | 97 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_EUL) 98 | printf(" Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x62_msg.node_data[i].eul_r, imu_0x62_msg.node_data[i].eul_p, imu_0x62_msg.node_data[i].eul_y); 99 | 100 | if(imu_0x62_msg.node_data[i].bitmap & BIT_VALID_QUAT) 101 | printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu_0x62_msg.node_data[i].quat_w, imu_0x62_msg.node_data[i].quat_x, imu_0x62_msg.node_data[i].quat_y, imu_0x62_msg.node_data[i].quat_z); 102 | 103 | #endif 104 | } 105 | 106 | } 107 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H 26 | #define IMU_FILTER_MADWICK_IMU_FILTER_ROS_H 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include "tf2_ros/transform_broadcaster.h" 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "imu_filter_madgwick/imu_filter.h" 39 | #include "imu_filter_madgwick/ImuFilterMadgwickConfig.h" 40 | 41 | class ImuFilterRos 42 | { 43 | typedef sensor_msgs::Imu ImuMsg; 44 | typedef sensor_msgs::MagneticField MagMsg; 45 | typedef geometry_msgs::Vector3Stamped MagVectorMsg; 46 | 47 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 48 | typedef message_filters::Synchronizer Synchronizer; 49 | typedef message_filters::Subscriber ImuSubscriber; 50 | typedef message_filters::Subscriber MagSubscriber; 51 | typedef message_filters::Subscriber MagVectorSubscriber; 52 | 53 | typedef imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig; 54 | typedef dynamic_reconfigure::Server FilterConfigServer; 55 | 56 | public: 57 | 58 | ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private); 59 | virtual ~ImuFilterRos(); 60 | 61 | private: 62 | 63 | // **** ROS-related 64 | 65 | ros::NodeHandle nh_; 66 | ros::NodeHandle nh_private_; 67 | 68 | boost::shared_ptr imu_subscriber_; 69 | boost::shared_ptr mag_subscriber_; 70 | boost::shared_ptr sync_; 71 | 72 | // Adapter to support the use_magnetic_field_msg param. 73 | boost::shared_ptr vector_mag_subscriber_; 74 | ros::Publisher mag_republisher_; 75 | 76 | ros::Publisher rpy_filtered_debug_publisher_; 77 | ros::Publisher rpy_raw_debug_publisher_; 78 | ros::Publisher imu_publisher_; 79 | tf2_ros::TransformBroadcaster tf_broadcaster_; 80 | 81 | boost::shared_ptr config_server_; 82 | ros::Timer check_topics_timer_; 83 | 84 | // **** paramaters 85 | WorldFrame::WorldFrame world_frame_; 86 | bool use_mag_; 87 | bool use_magnetic_field_msg_; 88 | bool stateless_; 89 | bool publish_tf_; 90 | bool reverse_tf_; 91 | std::string fixed_frame_; 92 | std::string imu_frame_; 93 | double constant_dt_; 94 | bool publish_debug_topics_; 95 | bool remove_gravity_vector_; 96 | geometry_msgs::Vector3 mag_bias_; 97 | double orientation_variance_; 98 | 99 | // **** state variables 100 | boost::mutex mutex_; 101 | bool initialized_; 102 | ros::Time last_time_; 103 | 104 | // **** filter implementation 105 | ImuFilter filter_; 106 | 107 | // **** member functions 108 | void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, 109 | const MagMsg::ConstPtr& mav_msg); 110 | 111 | void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw); 112 | 113 | void imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg); 114 | 115 | void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw); 116 | void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw); 117 | 118 | void publishRawMsg(const ros::Time& t, 119 | float roll, float pitch, float yaw); 120 | 121 | void reconfigCallback(FilterConfig& config, uint32_t level); 122 | void checkTopicsTimerCallback(const ros::TimerEvent&); 123 | }; 124 | 125 | #endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H 126 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/test/madgwick_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "test_helpers.h" 4 | 5 | #define FILTER_ITERATIONS 10000 6 | 7 | 8 | template 9 | void filterStationary( 10 | float Ax, float Ay, float Az, 11 | float Mx, float My, float Mz, 12 | double& q0, double& q1, double& q2, double& q3) { 13 | float dt = 0.1; 14 | float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0) 15 | 16 | ImuFilter filter; 17 | filter.setDriftBiasGain(0.0); 18 | filter.setAlgorithmGain(0.1); 19 | 20 | // initialize with some orientation 21 | filter.setOrientation(q0,q1,q2,q3); 22 | filter.setWorldFrame(FRAME); 23 | 24 | for (int i = 0; i < FILTER_ITERATIONS; i++) { 25 | filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt); 26 | } 27 | 28 | filter.getOrientation(q0,q1,q2,q3); 29 | } 30 | 31 | 32 | template 33 | void filterStationary( 34 | float Ax, float Ay, float Az, 35 | double& q0, double& q1, double& q2, double& q3) { 36 | float dt = 0.1; 37 | float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0) 38 | 39 | ImuFilter filter; 40 | filter.setDriftBiasGain(0.0); 41 | filter.setAlgorithmGain(0.1); 42 | 43 | // initialize with some orientation 44 | filter.setOrientation(q0,q1,q2,q3); 45 | filter.setWorldFrame(FRAME); 46 | 47 | for (int i = 0; i < FILTER_ITERATIONS; i++) { 48 | filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt); 49 | } 50 | 51 | filter.getOrientation(q0,q1,q2,q3); 52 | } 53 | 54 | 55 | #define TEST_STATIONARY_ENU(in_am, exp_result) \ 56 | TEST(MadgwickTest, Stationary_ENU_ ## in_am){ \ 57 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 58 | filterStationary(in_am, q0, q1, q2, q3); \ 59 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 60 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 61 | TEST(MadgwickTest, Stationary_ENU_NM_ ## in_am){ \ 62 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 63 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 64 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 65 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 66 | 67 | #define TEST_STATIONARY_NED(in_am, exp_result) \ 68 | TEST(MadgwickTest, Stationary_NED_ ## in_am){ \ 69 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 70 | filterStationary(in_am, q0, q1, q2, q3); \ 71 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 72 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 73 | TEST(MadgwickTest, Stationary_NED_NM_ ## in_am){ \ 74 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 75 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 76 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 77 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 78 | 79 | #define TEST_STATIONARY_NWU(in_am, exp_result) \ 80 | TEST(MadgwickTest, Stationary_NWU_ ## in_am){ \ 81 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 82 | filterStationary(in_am, q0, q1, q2, q3); \ 83 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 84 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 85 | TEST(MadgwickTest, Stationary_NWU_NM_ ## in_am){ \ 86 | double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \ 87 | filterStationary(ACCEL_ONLY(in_am), q0, q1, q2, q3); \ 88 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 89 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 90 | 91 | TEST_STATIONARY_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180) 92 | TEST_STATIONARY_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY) 93 | TEST_STATIONARY_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU) 94 | TEST_STATIONARY_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU) 95 | 96 | TEST_STATIONARY_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY) 97 | TEST_STATIONARY_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120) 98 | TEST_STATIONARY_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90) 99 | TEST_STATIONARY_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU) 100 | TEST_STATIONARY_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU) 101 | 102 | TEST_STATIONARY_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY) 103 | TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180) 104 | TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED) 105 | TEST_STATIONARY_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED) 106 | 107 | 108 | 109 | TEST(MadgwickTest, TestQuatEqNoZ) { 110 | 111 | ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90)); 112 | ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180)); 113 | 114 | } 115 | 116 | 117 | 118 | int main(int argc, char **argv){ 119 | testing::InitGoogleTest(&argc, argv); 120 | return RUN_ALL_TESTS(); 121 | } 122 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/test/stateless_orientation_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test_helpers.h" 3 | 4 | 5 | template 6 | bool computeOrientation( 7 | float Ax, float Ay, float Az, 8 | float Mx, float My, float Mz, 9 | double& q0, double& q1, double& q2, double& q3) { 10 | 11 | geometry_msgs::Vector3 A, M; 12 | geometry_msgs::Quaternion orientation; 13 | A.x = Ax; A.y = Ay; A.z = Az; 14 | M.x = Mx; M.y = My; M.z = Mz; 15 | 16 | bool res = StatelessOrientation::computeOrientation(FRAME, A, M, orientation); 17 | 18 | q0 = orientation.w; 19 | q1 = orientation.x; 20 | q2 = orientation.y; 21 | q3 = orientation.z; 22 | 23 | return res; 24 | } 25 | 26 | template 27 | bool computeOrientation( 28 | float Ax, float Ay, float Az, 29 | double& q0, double& q1, double& q2, double& q3) { 30 | 31 | geometry_msgs::Vector3 A; 32 | geometry_msgs::Quaternion orientation; 33 | A.x = Ax; A.y = Ay; A.z = Az; 34 | 35 | bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation); 36 | 37 | q0 = orientation.w; 38 | q1 = orientation.x; 39 | q2 = orientation.y; 40 | q3 = orientation.z; 41 | 42 | return res; 43 | } 44 | 45 | 46 | #define TEST_STATELESS_ENU(in_am, exp_result) \ 47 | TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){ \ 48 | double q0, q1, q2, q3; \ 49 | ASSERT_TRUE(computeOrientation(in_am, q0, q1, q2, q3)); \ 50 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 51 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 52 | TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){ \ 53 | double q0, q1, q2, q3; \ 54 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 55 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 56 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 57 | 58 | #define TEST_STATELESS_NED(in_am, exp_result) \ 59 | TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){ \ 60 | double q0, q1, q2, q3; \ 61 | ASSERT_TRUE(computeOrientation(in_am, q0, q1, q2, q3)); \ 62 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 63 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 64 | TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){ \ 65 | double q0, q1, q2, q3; \ 66 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 67 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 68 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 69 | 70 | #define TEST_STATELESS_NWU(in_am, exp_result) \ 71 | TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){ \ 72 | double q0, q1, q2, q3; \ 73 | ASSERT_TRUE(computeOrientation(in_am, q0, q1, q2, q3)); \ 74 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 75 | ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 76 | TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){ \ 77 | double q0, q1, q2, q3; \ 78 | ASSERT_TRUE(computeOrientation(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 79 | ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 80 | ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 81 | 82 | TEST_STATELESS_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY) 83 | TEST_STATELESS_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120) 84 | TEST_STATELESS_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90) 85 | TEST_STATELESS_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU) 86 | TEST_STATELESS_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU) 87 | 88 | 89 | TEST_STATELESS_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY) 90 | TEST_STATELESS_NED(AM_WEST_NORTH_DOWN, QUAT_MZ_90) 91 | TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED) 92 | TEST_STATELESS_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED) 93 | 94 | 95 | TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180) 96 | TEST_STATELESS_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY) 97 | TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU) 98 | TEST_STATELESS_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU) 99 | 100 | 101 | TEST(StatelessOrientationTest, Check_NoAccel){ 102 | double q0, q1, q2, q3; 103 | ASSERT_FALSE(computeOrientation( 104 | 0.0, 0.0, 0.0, 105 | 0.0, 0.0005, -0.0005, 106 | q0, q1, q2, q3)); 107 | } 108 | 109 | TEST(StatelessOrientationTest, Check_NoMag){ 110 | double q0, q1, q2, q3; 111 | ASSERT_FALSE(computeOrientation( 112 | 0.0, 0.0, 9.81, 113 | 0.0, 0.0, 0.0, 114 | q0, q1, q2, q3)); 115 | } 116 | 117 | 118 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_axes_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "imu_axes_visual.h" 32 | 33 | #include 34 | #include 35 | 36 | namespace rviz 37 | { 38 | 39 | ImuAxesVisual::ImuAxesVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node): 40 | orientation_axes_(NULL), 41 | scale_(0.15), quat_valid_(true) 42 | { 43 | scene_manager_ = scene_manager; 44 | 45 | // Ogre::SceneNode s form a tree, with each node storing the 46 | // transform (position and orientation) of itself relative to its 47 | // parent. Ogre does the math of combining those transforms when it 48 | // is time to render. 49 | // 50 | // Here we create a node to store the pose of the Imu's header frame 51 | // relative to the RViz fixed frame. 52 | frame_node_ = parent_node->createChildSceneNode(); 53 | } 54 | 55 | ImuAxesVisual::~ImuAxesVisual() 56 | { 57 | hide(); 58 | 59 | // Destroy the frame node since we don't need it anymore. 60 | scene_manager_->destroySceneNode(frame_node_); 61 | } 62 | 63 | void ImuAxesVisual::show() 64 | { 65 | if (!orientation_axes_) 66 | { 67 | orientation_axes_ = new Axes(scene_manager_, frame_node_); 68 | orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_)); 69 | orientation_axes_->setOrientation(orientation_); 70 | } 71 | } 72 | 73 | void ImuAxesVisual::hide() 74 | { 75 | if (orientation_axes_) 76 | { 77 | delete orientation_axes_; 78 | orientation_axes_ = NULL; 79 | } 80 | } 81 | 82 | void ImuAxesVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg) 83 | { 84 | if (checkQuaternionValidity(msg)) { 85 | if (!quat_valid_) { 86 | ROS_INFO("rviz_imu_plugin got valid quaternion, " 87 | "displaying true orientation"); 88 | quat_valid_ = true; 89 | } 90 | orientation_ = Ogre::Quaternion(msg->orientation.w, 91 | msg->orientation.x, 92 | msg->orientation.y, 93 | msg->orientation.z); 94 | } else { 95 | if (quat_valid_) { 96 | ROS_WARN("rviz_imu_plugin got invalid quaternion (%lf, %lf, %lf, %lf), " 97 | "will display neutral orientation instead", msg->orientation.w, 98 | msg->orientation.x,msg->orientation.y,msg->orientation.z); 99 | quat_valid_ = false; 100 | } 101 | // if quaternion is invalid, give a unit quat to Ogre 102 | orientation_ = Ogre::Quaternion(); 103 | } 104 | 105 | if (orientation_axes_) 106 | orientation_axes_->setOrientation(orientation_); 107 | } 108 | 109 | void ImuAxesVisual::setScale(float scale) 110 | { 111 | scale_ = scale; 112 | if (orientation_axes_) 113 | orientation_axes_->setScale(Ogre::Vector3(scale_, scale_, scale_)); 114 | } 115 | 116 | void ImuAxesVisual::setFramePosition(const Ogre::Vector3& position) 117 | { 118 | frame_node_->setPosition(position); 119 | } 120 | 121 | void ImuAxesVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 122 | { 123 | frame_node_->setOrientation(orientation); 124 | } 125 | 126 | inline bool ImuAxesVisual::checkQuaternionValidity( 127 | const sensor_msgs::Imu::ConstPtr& msg) { 128 | 129 | double x = msg->orientation.x, 130 | y = msg->orientation.y, 131 | z = msg->orientation.z, 132 | w = msg->orientation.w; 133 | // OGRE can handle unnormalized quaternions, but quat's length extremely small; 134 | // this may indicate that invalid (0, 0, 0, 0) quat is passed, this will lead ogre 135 | // to crash unexpectly 136 | if ( std::sqrt( x*x + y*y + z*z + w*w ) < 0.0001 ) { 137 | return false; 138 | } 139 | return true; 140 | } 141 | 142 | 143 | } // end namespace rviz 144 | 145 | -------------------------------------------------------------------------------- /imu_tool/imu_filter_madgwick/src/stateless_orientation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, CCNY Robotics Lab 3 | * Ivan Dryanovski 4 | * 5 | * http://robotics.ccny.cuny.edu 6 | * 7 | * Based on implementation of Madgwick's IMU and AHRS algorithms. 8 | * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 9 | * 10 | * 11 | * This program is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #include "imu_filter_madgwick/stateless_orientation.h" 26 | #include 27 | #include 28 | #include 29 | 30 | template 31 | static inline void crossProduct( 32 | T ax, T ay, T az, 33 | T bx, T by, T bz, 34 | T& rx, T& ry, T& rz) { 35 | rx = ay*bz - az*by; 36 | ry = az*bx - ax*bz; 37 | rz = ax*by - ay*bx; 38 | } 39 | 40 | 41 | template 42 | static inline T normalizeVector(T& vx, T& vy, T& vz) { 43 | T norm = sqrt(vx*vx + vy*vy + vz*vz); 44 | T inv = 1.0 / norm; 45 | vx *= inv; 46 | vy *= inv; 47 | vz *= inv; 48 | return norm; 49 | 50 | } 51 | 52 | 53 | bool StatelessOrientation::computeOrientation( 54 | WorldFrame::WorldFrame frame, 55 | geometry_msgs::Vector3 A, 56 | geometry_msgs::Vector3 E, 57 | geometry_msgs::Quaternion& orientation) { 58 | 59 | float Hx, Hy, Hz; 60 | float Mx, My, Mz; 61 | float normH; 62 | 63 | // A: pointing up 64 | float Ax = A.x, Ay = A.y, Az = A.z; 65 | 66 | // E: pointing down/north 67 | float Ex = E.x, Ey = E.y, Ez = E.z; 68 | 69 | // H: vector horizontal, pointing east 70 | // H = E x A 71 | crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz); 72 | 73 | // normalize H 74 | normH = normalizeVector(Hx, Hy, Hz); 75 | if (normH < 1E-7) { 76 | // device is close to free fall (or in space?), or close to 77 | // magnetic north pole. 78 | // mag in T => Threshold 1E-7, typical values are > 1E-5. 79 | return false; 80 | } 81 | 82 | // normalize A 83 | normalizeVector(Ax, Ay, Az); 84 | 85 | // M: vector horizontal, pointing north 86 | // M = A x H 87 | crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz); 88 | 89 | // Create matrix for basis transformation 90 | tf2::Matrix3x3 R; 91 | switch (frame) { 92 | case WorldFrame::NED: 93 | // vector space world W: 94 | // Basis: bwx (1,0,0) north, bwy (0,1,0) east, bwz (0,0,1) down 95 | // vector space local L: 96 | // Basis: M ,H, -A 97 | // W(1,0,0) => L(M) 98 | // W(0,1,0) => L(H) 99 | // W(0,0,1) => L(-A) 100 | 101 | // R: Transform Matrix local => world equals basis of L, because basis of W is I 102 | R[0][0] = Mx; R[0][1] = Hx; R[0][2] = -Ax; 103 | R[1][0] = My; R[1][1] = Hy; R[1][2] = -Ay; 104 | R[2][0] = Mz; R[2][1] = Hz; R[2][2] = -Az; 105 | break; 106 | 107 | case WorldFrame::NWU: 108 | // vector space world W: 109 | // Basis: bwx (1,0,0) north, bwy (0,1,0) west, bwz (0,0,1) up 110 | // vector space local L: 111 | // Basis: M ,H, -A 112 | // W(1,0,0) => L(M) 113 | // W(0,1,0) => L(-H) 114 | // W(0,0,1) => L(A) 115 | 116 | // R: Transform Matrix local => world equals basis of L, because basis of W is I 117 | R[0][0] = Mx; R[0][1] = -Hx; R[0][2] = Ax; 118 | R[1][0] = My; R[1][1] = -Hy; R[1][2] = Ay; 119 | R[2][0] = Mz; R[2][1] = -Hz; R[2][2] = Az; 120 | break; 121 | 122 | default: 123 | case WorldFrame::ENU: 124 | // vector space world W: 125 | // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up 126 | // vector space local L: 127 | // Basis: H, M , A 128 | // W(1,0,0) => L(H) 129 | // W(0,1,0) => L(M) 130 | // W(0,0,1) => L(A) 131 | 132 | // R: Transform Matrix local => world equals basis of L, because basis of W is I 133 | R[0][0] = Hx; R[0][1] = Mx; R[0][2] = Ax; 134 | R[1][0] = Hy; R[1][1] = My; R[1][2] = Ay; 135 | R[2][0] = Hz; R[2][1] = Mz; R[2][2] = Az; 136 | break; 137 | } 138 | 139 | // Matrix.getRotation assumes vector rotation, but we're using 140 | // coordinate systems. Thus negate rotation angle (inverse). 141 | tf2::Quaternion q; 142 | R.getRotation(q); 143 | tf2::convert(q.inverse(), orientation); 144 | return true; 145 | } 146 | 147 | 148 | bool StatelessOrientation::computeOrientation( 149 | WorldFrame::WorldFrame frame, 150 | geometry_msgs::Vector3 A, 151 | geometry_msgs::Quaternion& orientation) { 152 | 153 | // This implementation could be optimized regarding speed. 154 | 155 | // magnetic Field E must not be parallel to A, 156 | // choose an arbitrary orthogonal vector 157 | geometry_msgs::Vector3 E; 158 | if (fabs(A.x) > 0.1 || fabs(A.y) > 0.1) { 159 | E.x = A.y; 160 | E.y = A.x; 161 | E.z = 0.0; 162 | } else if (fabs(A.z) > 0.1) { 163 | E.x = 0.0; 164 | E.y = A.z; 165 | E.z = A.y; 166 | } else { 167 | // free fall 168 | return false; 169 | } 170 | 171 | return computeOrientation(frame, A, E, orientation); 172 | } 173 | -------------------------------------------------------------------------------- /racecar/rviz/test_laser.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /LaserScan1 11 | - /Axes1 12 | - /Axes1/Status1 13 | - /TF1/Frames1 14 | Splitter Ratio: 0.5 15 | Tree Height: 490 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679016 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: LaserScan 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 26 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 10 60 | Min Value: -10 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/LaserScan 65 | Color: 255; 255; 255 66 | Color Transformer: Intensity 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Max Intensity: 2319 72 | Min Color: 0; 0; 0 73 | Min Intensity: 2 74 | Name: LaserScan 75 | Position Transformer: XYZ 76 | Queue Size: 10 77 | Selectable: true 78 | Size (Pixels): 3 79 | Size (m): 0.00999999978 80 | Style: Points 81 | Topic: /scan 82 | Unreliable: false 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | - Class: rviz/Axes 87 | Enabled: true 88 | Length: 0.5 89 | Name: Axes 90 | Radius: 0.0500000007 91 | Reference Frame: 92 | Value: true 93 | - Class: rviz/TF 94 | Enabled: true 95 | Frame Timeout: 15 96 | Frames: 97 | All Enabled: true 98 | Marker Scale: 1 99 | Name: TF 100 | Show Arrows: true 101 | Show Axes: true 102 | Show Names: true 103 | Tree: 104 | {} 105 | Update Interval: 1 106 | Value: true 107 | Enabled: true 108 | Global Options: 109 | Background Color: 48; 48; 48 110 | Default Light: true 111 | Fixed Frame: laser 112 | Frame Rate: 30 113 | Name: root 114 | Tools: 115 | - Class: rviz/Interact 116 | Hide Inactive Objects: true 117 | - Class: rviz/MoveCamera 118 | - Class: rviz/Select 119 | - Class: rviz/FocusCamera 120 | - Class: rviz/Measure 121 | - Class: rviz/SetInitialPose 122 | Topic: /initialpose 123 | - Class: rviz/SetGoal 124 | Topic: /move_base_simple/goal 125 | - Class: rviz/PublishPoint 126 | Single click: true 127 | Topic: /clicked_point 128 | Value: true 129 | Views: 130 | Current: 131 | Class: rviz/Orbit 132 | Distance: 10.5115833 133 | Enable Stereo Rendering: 134 | Stereo Eye Separation: 0.0599999987 135 | Stereo Focal Distance: 1 136 | Swap Stereo Eyes: false 137 | Value: false 138 | Focal Point: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Focal Shape Fixed Size: true 143 | Focal Shape Size: 0.0500000007 144 | Invert Z Axis: false 145 | Name: Current View 146 | Near Clip Distance: 0.00999999978 147 | Pitch: 1.06979573 148 | Target Frame: 149 | Value: Orbit (rviz) 150 | Yaw: 2.66859388 151 | Saved: ~ 152 | Window Geometry: 153 | Displays: 154 | collapsed: false 155 | Height: 773 156 | Hide Left Dock: false 157 | Hide Right Dock: false 158 | QMainWindow State: 000000ff00000000fd00000004000000000000017000000279fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000279000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000279fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000279000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054300000040fc0100000002fb0000000800540069006d00650100000000000005430000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002b80000027900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 159 | Selection: 160 | collapsed: false 161 | Time: 162 | collapsed: false 163 | Tool Properties: 164 | collapsed: false 165 | Views: 166 | collapsed: false 167 | Width: 1347 168 | X: 152 169 | Y: 136 170 | -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_acc_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "imu_acc_visual.h" 32 | 33 | namespace rviz 34 | { 35 | 36 | ImuAccVisual::ImuAccVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node): 37 | acc_vector_(NULL), 38 | arrow_length_(9.81), 39 | arrow_radius_(0.50), 40 | head_length_(1.00), 41 | head_radius_(1.00), 42 | scale_(0.05), 43 | alpha_(1.0), 44 | color_(1.0, 1.0, 0.0), 45 | derotated_(true) 46 | { 47 | scene_manager_ = scene_manager; 48 | 49 | // Ogre::SceneNode s form a tree, with each node storing the 50 | // transform (position and orientation) of itself relative to its 51 | // parent. Ogre does the math of combining those transforms when it 52 | // is time to render. 53 | // 54 | // Here we create a node to store the pose of the Imu's header frame 55 | // relative to the RViz fixed frame. 56 | frame_node_ = parent_node->createChildSceneNode(); 57 | } 58 | 59 | ImuAccVisual::~ImuAccVisual() 60 | { 61 | hide(); 62 | 63 | // Destroy the frame node since we don't need it anymore. 64 | scene_manager_->destroySceneNode(frame_node_); 65 | } 66 | 67 | void ImuAccVisual::show() 68 | { 69 | if (!acc_vector_) 70 | { 71 | acc_vector_ = new Arrow(scene_manager_, frame_node_); 72 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_); 73 | acc_vector_->setDirection(direction_); 74 | acc_vector_->set( 75 | arrow_length_ * scale_, 76 | arrow_radius_ * scale_, 77 | head_length_ * scale_, 78 | head_radius_ * scale_); 79 | } 80 | } 81 | 82 | void ImuAccVisual::hide() 83 | { 84 | if (acc_vector_) 85 | { 86 | delete acc_vector_; 87 | acc_vector_ = NULL; 88 | } 89 | } 90 | 91 | void ImuAccVisual::setMessage(const sensor_msgs::Imu::ConstPtr& msg) 92 | { 93 | direction_ = Ogre::Vector3(msg->linear_acceleration.x, 94 | msg->linear_acceleration.y, 95 | msg->linear_acceleration.z); 96 | 97 | 98 | // Rotate the acceleration vector by the IMU orientation. This makes 99 | // sense since the visualization of the IMU is also rotated by the 100 | // orientation. In this way, both appear in the inertial frame. 101 | if (derotated_) 102 | { 103 | Ogre::Quaternion orientation(msg->orientation.w, 104 | msg->orientation.x, 105 | msg->orientation.y, 106 | msg->orientation.z); 107 | 108 | direction_ = orientation * direction_; 109 | } 110 | 111 | arrow_length_ = sqrt( 112 | msg->linear_acceleration.x * msg->linear_acceleration.x + 113 | msg->linear_acceleration.y * msg->linear_acceleration.y + 114 | msg->linear_acceleration.z * msg->linear_acceleration.z); 115 | 116 | if (acc_vector_) 117 | { 118 | acc_vector_->setDirection(direction_); 119 | acc_vector_->set( 120 | arrow_length_ * scale_, 121 | arrow_radius_ * scale_, 122 | head_length_ * scale_, 123 | head_radius_ * scale_); 124 | } 125 | } 126 | 127 | void ImuAccVisual::setScale(float scale) 128 | { 129 | scale_ = scale; 130 | if (acc_vector_) 131 | { 132 | acc_vector_->setDirection(direction_); 133 | acc_vector_->set( 134 | arrow_length_ * scale_, 135 | arrow_radius_ * scale_, 136 | head_length_ * scale_, 137 | head_radius_ * scale_); 138 | } 139 | } 140 | 141 | void ImuAccVisual::setColor(const QColor& color) 142 | { 143 | color_ = color; 144 | if (acc_vector_) 145 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(), alpha_); 146 | } 147 | 148 | void ImuAccVisual::setAlpha(float alpha) 149 | { 150 | alpha_ = alpha; 151 | if (acc_vector_) 152 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_); 153 | } 154 | 155 | void ImuAccVisual::setDerotated(bool derotated) 156 | { 157 | derotated_ = derotated; 158 | if (acc_vector_) 159 | acc_vector_->setColor(color_.redF(), color_.greenF(), color_.blueF(),alpha_); 160 | } 161 | 162 | void ImuAccVisual::setFramePosition(const Ogre::Vector3& position) 163 | { 164 | frame_node_->setPosition(position); 165 | } 166 | 167 | void ImuAccVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 168 | { 169 | frame_node_->setOrientation(orientation); 170 | } 171 | 172 | } // end namespace rviz 173 | 174 | -------------------------------------------------------------------------------- /multi_goal/src/goal_loop.py: -------------------------------------------------------------------------------- 1 | #coding:utf-8 2 | from geometry_msgs.msg import PoseStamped, Pose 3 | #!/usr/bin/env python 4 | import rospy 5 | import string 6 | import math 7 | import time 8 | import sys 9 | import csv 10 | import numpy as np 11 | from nav_msgs.msg import Odometry 12 | from std_msgs.msg import String, Float64 13 | from move_base_msgs.msg import MoveBaseActionResult 14 | from actionlib_msgs.msg import GoalStatusArray 15 | from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped 16 | 17 | #repeat the route use for test 18 | #功能:循环导航 19 | #csv要求 第一个点是起点下一个点，终点是起点 1230 20 | class MultiGoals: 21 | def __init__(self, goalListX, goalListY, retry, map_frame): 22 | self.retry = 1 23 | if(self.retry == 1): 24 | #self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=1) 25 | self.pub = rospy.Publisher( 26 | 'move_base_simple/goal', PoseStamped, queue_size=10) 27 | #self.pose_ekf = rospy.Subscriber('/odometry/filtered',Odometry,self.getPose_ekf,queue_size=10) 28 | self.pose_amcl = rospy.Subscriber( 29 | "/amcl_pose", PoseWithCovarianceStamped, self.getPose_amcl, queue_size=10) 30 | # params & variables 31 | self.pub_final = rospy.Publisher('/arrfinal', Float64, queue_size=1) 32 | self.goalListX = goalListX 33 | self.goalListY = goalListY 34 | self.goalListW = goalListW 35 | self.goalListZ = goalListZ 36 | self.kx = 0 37 | self.ky = 0 38 | self.gx = 0 39 | self.gy = 0 40 | self.flag = 1 41 | self.MIN_DISTANCE = 1.5 # min distance of the judge between the goal and odometrypose 42 | self.LONG = len(self.goalListX) 43 | self.goalId = 0 44 | self.count = 0 45 | self.start_time = 0 46 | self.pubfinal = False 47 | self.goalMsg = PoseStamped() 48 | self.goalMsg.header.frame_id = map_frame 49 | 50 | time.sleep(1) 51 | self.goalMsg.header.stamp = rospy.Time.now() 52 | self.goalMsg.pose.position.x = self.goalListX[self.goalId] 53 | self.goalMsg.pose.position.y = self.goalListY[self.goalId] 54 | self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId] 55 | self.goalMsg.pose.orientation.w = self.goalListW[self.goalId] 56 | self.pub.publish(self.goalMsg) 57 | # print(self.goalMsg) 58 | self.start_time = rospy.get_time() 59 | rospy.loginfo( 60 | "Initial goal published! Goal ID is: %d", self.goalId) 61 | self.goalId = self.goalId + 1 62 | 63 | def statusCB(self): 64 | if self.pubfinal == True: 65 | self.pub_final.publish(1.0) 66 | print("the final goal") 67 | 68 | self.gx = self.goalListX[self.goalId-1] if(self.goalId != 0) else self.goalListX[self.goalId] 69 | self.gy = self.goalListY[self.goalId-1] if(self.goalId != 0) else self.goalListY[self.goalId] 70 | 71 | self.dist = self.distance(self.kx, self.ky, self.gx, self.gy) 72 | 73 | if self.dist < self.MIN_DISTANCE and self.flag == 1: 74 | 75 | finish_time = rospy.get_time() 76 | interval = finish_time - self.start_time 77 | print(interval) 78 | if self.goalId == self.LONG: 79 | self.goalId = 0 80 | self.goalMsg.header.stamp = rospy.Time.now() 81 | self.goalMsg.pose.position.x = self.goalListX[self.goalId] 82 | self.goalMsg.pose.position.y = self.goalListY[self.goalId] 83 | self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId] 84 | self.goalMsg.pose.orientation.w = self.goalListW[self.goalId] 85 | self.pub.publish(self.goalMsg) 86 | rospy.loginfo( 87 | "Initial goal published! Goal ID is: %d", self.goalId) 88 | rospy.loginfo("intostatusCB") 89 | self.count = self.count+1 90 | print(self.count) 91 | 92 | if self.goalId < (len(self.goalListX)): 93 | self.goalId = self.goalId + 1 94 | 95 | else: 96 | self.goalId = 0 97 | print("final") 98 | 99 | def getPose_ekf(self, data): 100 | self.kx = data.pose.pose.position.x 101 | self.ky = data.pose.pose.position.y 102 | self.statusCB() 103 | 104 | def getPose_amcl(self, data): 105 | self.kx = data.pose.pose.position.x 106 | self.ky = data.pose.pose.position.y 107 | self.statusCB() 108 | 109 | def distance(self, kx, ky, gx, gy): 110 | try: 111 | return math.sqrt((kx-gx)**2+(ky-gy)**2) 112 | except: 113 | return None 114 | 115 | 116 | 117 | if __name__ == "__main__": 118 | try: 119 | # ROS Init 120 | rospy.init_node('multi_goals', anonymous=True) 121 | retry = 1 122 | goalList = [] 123 | goalListX=[] 124 | goalListY=[] 125 | goalListZ=[] 126 | goalListW=[] 127 | map_frame = rospy.get_param('~map_frame', 'map' ) 128 | 129 | with open('test.csv', 'r') as f: 130 | reader = csv.reader(f) 131 | 132 | for cols in reader: 133 | goalList.append([float(value) for value in cols]) 134 | 135 | goalList = np.array(goalList) 136 | print("read suc!!") 137 | goalListX = goalList[:,0] 138 | goalListY = goalList[:,1] 139 | goalListZ = goalList[:,2] 140 | goalListW = goalList[:,3] 141 | 142 | 143 | if len(goalListX) == len(goalListY) & len(goalListY) >= 1: 144 | # Constract MultiGoals Obj 145 | rospy.loginfo("Multi Goals Executing...") 146 | mg = MultiGoals(goalListX, goalListY, retry, map_frame) 147 | rospy.spin() 148 | else: 149 | rospy.loginfo("Lengths of goal lists are not the same") 150 | except KeyboardInterrupt: 151 | print("shutting down") 152 | 153 | 154 | -------------------------------------------------------------------------------- /racecar/src/racecar_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | 7 | import sys, select, termios, tty 8 | import time 9 | 10 | msg = """ 11 | Control Your racecar! 12 | Reading from the keyboard and Publishing to Twist! 13 | --------------------------- 14 | Moving around: 15 | u i o 16 | j k l 17 | m , . 18 | 19 | For Holonomic mode (strafing), hold down the shift key: 20 | --------------------------- 21 | U I O 22 | J K L 23 | M < > 24 | 25 | space key, k : force stop 26 | w/x: shift the middle pos of throttle by +/- 5 pwm 27 | a/d: shift the middle pos of steering by +/- 2 pwm 28 | CTRL-C to quit 29 | """ 30 | 31 | moveBindings = { 32 | 'i':(1,0), 33 | 'o':(1,-1), 34 | 'j':(0,1), 35 | 'l':(0,-1), 36 | 'u':(1,1), 37 | ',':(-1,0), 38 | '.':(-1,-1), 39 | 'm':(-1,1), 40 | 'I':(1,0), 41 | 'O':(1,-1), 42 | 'J':(0,1), 43 | 'L':(0,-1), 44 | 'U':(1,1), 45 | '<':(-1,0), 46 | '>':(-1,-1), 47 | 'M':(-1,1), 48 | } 49 | 50 | 51 | def getKey(): 52 | tty.setraw(sys.stdin.fileno()) 53 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 54 | if rlist: 55 | key = sys.stdin.read(1) 56 | else: 57 | key = '' 58 | 59 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 60 | return key 61 | 62 | 63 | def vels(speed,turn): 64 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 65 | 66 | if __name__=="__main__": 67 | settings = termios.tcgetattr(sys.stdin) 68 | 69 | rospy.init_node('racecar_teleop') 70 | pub = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5) 71 | 72 | speed_start_value =150 # can roll 73 | turn_start_value = 60 # max value 74 | speed_mid = 1500 75 | turn_mid = 90 76 | speed_bias = 0 77 | turn_bias = 0 78 | speed_add_once = 5 79 | turn_add_once = 2 80 | control_speed = speed_mid 81 | control_turn = turn_mid 82 | speed_dir = 0 83 | last_speed_dir = 0 84 | last_turn_dir = 0 85 | last_control_speed = control_speed 86 | last_control_turn = control_turn 87 | 88 | run = 0 89 | 90 | try: 91 | while(1): 92 | key = getKey() 93 | twist = Twist() 94 | if key in moveBindings.keys(): 95 | run = 1 96 | 97 | #print "key =",key 98 | speed_dir = moveBindings[key][0] 99 | if(speed_dir!=0 and speed_dir + last_speed_dir == 0):#Reverse 100 | control_speed = speed_mid 101 | last_speed_dir = speed_dir 102 | control_turn = turn_mid 103 | #print "Reverse" 104 | else: 105 | if(speed_dir > 0): 106 | control_speed = speed_dir * (speed_start_value + speed_bias) + speed_mid 107 | elif(speed_dir < 0): 108 | control_speed = speed_dir * (speed_start_value + speed_bias) + speed_mid - 140 109 | else: 110 | control_speed = 1500 111 | 112 | control_turn = moveBindings[key][1] * (turn_start_value + turn_bias)+ turn_mid 113 | last_speed_dir = speed_dir 114 | 115 | #print " " 116 | #print "speed_dir = ",speed_dir 117 | #print "control_speed = ",control_speed 118 | #print "control_turn = ",control_turn 119 | last_control_speed = control_speed 120 | last_control_turn = control_turn 121 | elif key == ' ' or key == 'k' : 122 | speed_dir = -speed_dir #for ESC Forward/Reverse with brake mode 123 | control_speed = last_control_speed * speed_dir 124 | control_turn = turn_mid 125 | run = 0 126 | elif key == 'w' : 127 | speed_bias += speed_add_once 128 | if(speed_bias >= 450): 129 | speed_bias = 450 130 | else: 131 | last_control_speed = last_control_speed+speed_add_once 132 | run = 0 133 | elif key == 's' : 134 | speed_bias -= speed_add_once 135 | last_control_speed = last_control_speed-speed_add_once 136 | if(speed_bias <= 0): 137 | speed_bias = 0 138 | run = 0 139 | elif key == 'a' : 140 | turn_bias += turn_add_once 141 | last_control_turn = last_control_turn+turn_add_once 142 | if(turn_bias >= 80): 143 | turn_bias = 80 144 | run = 0 145 | elif key == 'd' : 146 | turn_bias -= turn_add_once 147 | last_control_turn = last_control_turn-turn_add_once 148 | if(turn_bias <= 0): 149 | turn_bias = 0 150 | run = 0 151 | else: 152 | run = 0 153 | print (vels(control_speed,control_turn)) 154 | #print "speed_dir=",speed_dir,"last_speed_dir",last_speed_dir 155 | if(run == 1): 156 | twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 157 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 158 | #print "control_speed =",control_speed 159 | pub.publish(twist) 160 | else: 161 | twist.linear.x = speed_mid; twist.linear.y = 0; twist.linear.z = 0 162 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn_mid 163 | pub.publish(twist) 164 | 165 | if (key == '\x03'): #for ctrl + c exit 166 | break 167 | 168 | except: 169 | print ("error") 170 | 171 | finally: 172 | twist = Twist() 173 | twist.linear.x = speed_mid; twist.linear.y = 0; twist.linear.z = 0 174 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn_mid 175 | pub.publish(twist) 176 | 177 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 178 | -------------------------------------------------------------------------------- /encoder_driver/src/imu_encoder_mix.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #define PI 3.14159 22 | using namespace std; 23 | using namespace message_filters; 24 | using namespace sensor_msgs; 25 | ros::Time current_time,last_time; 26 | double x = 0.0; 27 | double y = 0.0; 28 | double s = 0.0; 29 | double th = 0.0; 30 | double vth = 0.0; 31 | double th_init = 0.0; 32 | static double theta_first= 0.0; 33 | static bool flag = true; 34 | ros::Publisher encoder_imu_pub; 35 | 36 | 37 | //从车身姿态获取航向角 38 | double getYawFromPose(const sensor_msgs::Imu::ConstPtr &carPose) { 39 | //odom=*odom_cb; 40 | //geometry_msgs::Pose carPose = odom.pose.pose; 41 | sensor_msgs::Imu imu=(*carPose); 42 | float x = imu.orientation.x; 43 | float y = imu.orientation.y; 44 | float z = imu.orientation.z; 45 | float w = imu.orientation.w; 46 | 47 | double tmp, yaw; 48 | tf::Quaternion q(x, y, z, w); 49 | tf::Matrix3x3 quaternion(q); 50 | quaternion.getRPY(tmp, tmp, yaw); 51 | 52 | return yaw; 53 | } 54 | double getrollFromPose(const sensor_msgs::Imu::ConstPtr &carPose) { 55 | //odom=*odom_cb; 56 | //geometry_msgs::Pose carPose = odom.pose.pose; 57 | sensor_msgs::Imu imu=(*carPose); 58 | float x = imu.orientation.x; 59 | float y = imu.orientation.y; 60 | float z = imu.orientation.z; 61 | float w = imu.orientation.w; 62 | 63 | double roll,pitch, yaw; 64 | tf::Quaternion q(x, y, z, w); 65 | tf::Matrix3x3 quaternion(q); 66 | quaternion.getRPY(roll, pitch, yaw); 67 | 68 | return roll; 69 | } 70 | double getpitchFromPose(const sensor_msgs::Imu::ConstPtr &carPose) { 71 | //odom=*odom_cb; 72 | //geometry_msgs::Pose carPose = odom.pose.pose; 73 | sensor_msgs::Imu imu=(*carPose); 74 | float x = imu.orientation.x; 75 | float y = imu.orientation.y; 76 | float z = imu.orientation.z; 77 | float w = imu.orientation.w; 78 | 79 | double roll,pitch, yaw; 80 | tf::Quaternion q(x, y, z, w); 81 | tf::Matrix3x3 quaternion(q); 82 | quaternion.getRPY(roll, pitch, yaw); 83 | 84 | return pitch; 85 | } 86 | void callback(const sensor_msgs::Imu::ConstPtr& imu_data,const nav_msgs::Odometry::ConstPtr& speed) 87 | { 88 | if(flag==true) 89 | { 90 | theta_first=getYawFromPose(imu_data); 91 | flag=false; 92 | } 93 | 94 | if(flag==false) 95 | { 96 | double v=(*speed).twist.twist.linear.x; 97 | double theta=getYawFromPose(imu_data); 98 | double roll=getrollFromPose(imu_data); 99 | double pitch=getpitchFromPose(imu_data); 100 | theta = theta - theta_first; 101 | 102 | th_init = theta_first*180/PI; 103 | th = theta*180/PI;//转换成角度 104 | 105 | // if(th_init > 0) {if(th < -180) th += 360;} 106 | // if(th_init < 0) {if(th > 180 ) th -= 360;} 107 | double vx=v*cos(th/180*PI); 108 | double vy=v*sin(th/180*PI); 109 | current_time=ros::Time::now(); 110 | double dt=(current_time-last_time).toSec(); 111 | double delta_x = vx* dt; 112 | double delta_y = vy* dt; 113 | double delta_th = vth * dt;//0 114 | ROS_INFO("v :%.4f",v); 115 | //ROS_INFO("theta:%.4f",theta*180/PI); 116 | ROS_INFO("theta:%.4f",th); 117 | 118 | 119 | x+=delta_x; 120 | y+=delta_y; 121 | s = sqrt(x*x+y*y); 122 | ROS_INFO("lenth:%.1f",s*100); 123 | //th += delta_th; 124 | //定义odom发布需要的转换戳和位置 125 | 126 | // tf::TransformBroadcaster odom_broadcaster; 127 | // geometry_msgs::TransformStamped odom_trans; 128 | // odom_trans.header.stamp=current_time; 129 | // odom_trans.header.frame_id="odom"; 130 | // odom_trans.child_frame_id="base_footprint"; 131 | 132 | // odom_trans.transform.translation.x = x; 133 | // odom_trans.transform.translation.y = y; 134 | // odom_trans.transform.translation.z = 0.0; 135 | // odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(theta); 136 | // //将角度转换成旋转矩阵 137 | 138 | // odom_broadcaster.sendTransform(odom_trans); 139 | 140 | //然后发送里程计信息 发布者odom_pub 141 | nav_msgs::Odometry odom; 142 | odom.header.stamp=current_time; 143 | odom.header.frame_id="odom"; 144 | odom.child_frame_id="base_footprint"; 145 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,theta); 146 | 147 | odom.pose.pose.position.x = x; 148 | odom.pose.pose.position.y = y; 149 | odom.pose.pose.position.z = 0.0; 150 | odom.pose.pose.orientation = odom_quat; 151 | 152 | odom.twist.twist.linear.x = vx; 153 | odom.twist.twist.linear.y = vy; 154 | odom.twist.twist.angular.z = 0; 155 | 156 | encoder_imu_pub.publish(odom); 157 | 158 | last_time=current_time; 159 | } 160 | // ros::spinOnce(); 161 | 162 | } 163 | 164 | int main(int argc,char** argv) 165 | { 166 | //启动节点订阅消息 167 | ros::init(argc,argv,"encoder_imu_mix"); 168 | ros::NodeHandle nh; 169 | message_filters::Subscriber imu_sub(nh,"/imu_data",100); 170 | message_filters::Subscriber encoder_sub(nh,"/encoder",20); 171 | ROS_INFO("-------"); 172 | //创建发布后融合的消息的节点 173 | encoder_imu_pub = nh.advertise("/encoder_imu_odom",10); 174 | //定义同步消息储存器 精确时间戳 大概时间戳 175 | //typedef sync_policies::ApproximateTime MySyncPolicy; 176 | typedef sync_policies::ApproximateTime MySyncPolicy; 177 | 178 | //typedef sync_policies::ExactTime MySyncPolicy; 179 | Synchronizer sync(MySyncPolicy(10),imu_sub,encoder_sub); 180 | sync.registerCallback(boost::bind(&callback,_1,_2)); 181 | ros::spin(); 182 | return 0; 183 | } 184 | //回调函数的创建 callback 185 | 186 | // biaoding -------------------------------------------------------------------------------- /imu_tool/rviz_imu_plugin/src/imu_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * Copyright (c) 2012, Ivan Dryanovski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Willow Garage, Inc. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef RVIZ_IMU_PLUGIN_IMU_DISPLAY_H 32 | #define RVIZ_IMU_PLUGIN_IMU_DISPLAY_H 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "imu_axes_visual.h" 53 | #include "imu_orientation_visual.h" 54 | #include "imu_acc_visual.h" 55 | 56 | namespace Ogre 57 | { 58 | class SceneNode; 59 | } 60 | 61 | namespace rviz 62 | { 63 | 64 | class ImuDisplay: public rviz::MessageFilterDisplay 65 | { 66 | Q_OBJECT 67 | public: 68 | 69 | ImuDisplay(); 70 | virtual ~ImuDisplay(); 71 | 72 | virtual void onInitialize(); 73 | virtual void onEnable(); 74 | virtual void onDisable(); 75 | virtual void reset(); 76 | virtual void createProperties(); 77 | 78 | void setTopic(const std::string& topic); 79 | const std::string& getTopic() { return topic_; } 80 | 81 | void setBoxEnabled(bool enabled); 82 | void setBoxScaleX(float x); 83 | void setBoxScaleY(float y); 84 | void setBoxScaleZ(float z); 85 | void setBoxColor(const Color& color); 86 | void setBoxAlpha(float alpha); 87 | 88 | void setAxesEnabled(bool enabled); 89 | void setAxesScale(float scale); 90 | 91 | void setAccEnabled(bool enabled); 92 | void setAccDerotated(bool derotated); 93 | void setAccScale(float scale); 94 | void setAccColor(const Color& color); 95 | void setAccAlpha(float alpha); 96 | 97 | bool getBoxEnabled() { return box_enabled_; } 98 | float getBoxScaleX() { return box_visual_->getScaleX(); } 99 | float getBoxScaleY() { return box_visual_->getScaleY(); } 100 | float getBoxScaleZ() { return box_visual_->getScaleZ(); } 101 | float getBoxAlpha() { return box_visual_->getAlpha(); } 102 | const QColor& getBoxColor() { return box_visual_->getColor(); } 103 | 104 | bool getAxesEnabled() { return axes_enabled_; } 105 | float getAxesScale() { return axes_visual_->getScale(); } 106 | 107 | bool getAccEnabled() { return acc_enabled_; } 108 | float getAccDerotated() { return acc_visual_->getDerotated(); } 109 | float getAccScale() { return acc_visual_->getScale(); } 110 | float getAccAlpha() { return acc_visual_->getAlpha(); } 111 | const QColor& getAccColor() { return acc_visual_->getColor(); } 112 | 113 | protected Q_SLOTS: 114 | 115 | void updateTop(); 116 | void updateBox(); 117 | void updateAxes(); 118 | void updateAcc(); 119 | 120 | private: 121 | 122 | // Property objects for user-editable properties. 123 | rviz::BoolProperty* fixed_frame_orientation_property_; 124 | 125 | rviz::Property* box_category_; 126 | rviz::Property* axes_category_; 127 | rviz::Property* acc_category_; 128 | 129 | // rviz::RosTopicProperty* topic_property_; 130 | rviz::BoolProperty* box_enabled_property_; 131 | rviz::FloatProperty* box_scale_x_property_; 132 | rviz::FloatProperty* box_scale_y_property_; 133 | rviz::FloatProperty* box_scale_z_property_; 134 | rviz::ColorProperty* box_color_property_; 135 | rviz::FloatProperty* box_alpha_property_; 136 | 137 | rviz::BoolProperty* axes_enabled_property_; 138 | rviz::FloatProperty* axes_scale_property_; 139 | 140 | rviz::BoolProperty* acc_enabled_property_; 141 | rviz::BoolProperty* acc_derotated_property_; 142 | rviz::FloatProperty* acc_scale_property_; 143 | rviz::ColorProperty* acc_color_property_; 144 | rviz::FloatProperty* acc_alpha_property_; 145 | 146 | // Differetn types of visuals 147 | ImuOrientationVisual * box_visual_; 148 | ImuAxesVisual * axes_visual_; 149 | ImuAccVisual * acc_visual_; 150 | 151 | // User-editable property variables. 152 | std::string topic_; 153 | bool fixed_frame_orientation_; 154 | bool box_enabled_; 155 | bool axes_enabled_; 156 | bool acc_enabled_; 157 | 158 | // A node in the Ogre scene tree to be the parent of all our visuals. 159 | Ogre::SceneNode* scene_node_; 160 | 161 | int messages_received_; 162 | 163 | // Function to handle an incoming ROS message. 164 | void processMessage( const sensor_msgs::Imu::ConstPtr& msg); 165 | 166 | }; 167 | 168 | } // end namespace rviz 169 | 170 | #endif // IMU_DISPLAY_H 171 | 172 | -------------------------------------------------------------------------------- /serial_imu/src/serial_imu.cpp: -------------------------------------------------------------------------------- 1 | //serial_imu.cpp 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #ifdef __cplusplus 18 | extern "C"{ 19 | #endif 20 | 21 | #include 22 | #include 23 | #include "packet.h" 24 | #include "imu_data_decode.h" 25 | 26 | #define IMU_SERIAL "/dev/ttyUSB0" 27 | #define BAUD (115200) 28 | #define GRA_ACC (9.8) 29 | #define DEG_TO_RAD (0.01745329) 30 | #define BUF_SIZE 1024 31 | 32 | int imu_data_decode_init(void); 33 | typedef void (*on_data_received_event)(packet_t *ptr); 34 | void packet_decode_init(packet_t *pkt, on_data_received_event rx_handler); 35 | uint32_t packet_decode(uint8_t); 36 | void publish_0x91_data(id0x91_t *data, serial_imu::Imu_0x91_msg *data_imu); 37 | void publish_imu_data(id0x91_t *data, sensor_msgs::Imu *imu_data); 38 | void publish_0x62_data(id0x62_t *data, serial_imu::Imu_0x62_msg *data_imu); 39 | 40 | 41 | #ifdef __cplusplus 42 | } 43 | #endif 44 | 45 | static int frame_rate; 46 | 47 | static uint8_t buf[2048]; 48 | 49 | void timer(int sig) 50 | { 51 | if(SIGALRM == sig) 52 | { 53 | frame_rate = frame_count; 54 | frame_count = 0; 55 | alarm(1); 56 | } 57 | } 58 | 59 | int main(int argc, char** argv) 60 | { 61 | ros::init(argc, argv, "serial_imu"); 62 | ros::NodeHandle n; 63 | 64 | ros::Publisher IMU_pub = n.advertise("/IMU_data", 20); 65 | ros::Publisher Imu_0x91_pub = n.advertise("/imu_0x91_package", 10); 66 | ros::Publisher Imu_0x62_pub = n.advertise("/imu_0x62_package", 10); 67 | 68 | serial::Serial sp; 69 | 70 | serial::Timeout to = serial::Timeout::simpleTimeout(100); 71 | 72 | sp.setPort(IMU_SERIAL); 73 | 74 | sp.setBaudrate(BAUD); 75 | 76 | sp.setTimeout(to); 77 | 78 | 79 | imu_data_decode_init(); 80 | signal(SIGALRM,timer); 81 | 82 | try 83 | { 84 | sp.open(); 85 | } 86 | catch(serial::IOException& e) 87 | { 88 | ROS_ERROR_STREAM("Unable to open port."); 89 | return -1; 90 | } 91 | 92 | if(sp.isOpen()) 93 | { 94 | ROS_INFO_STREAM("/dev/ttyUSB0 is opened."); 95 | } 96 | else 97 | { 98 | return -1; 99 | } 100 | 101 | alarm(1); 102 | 103 | ros::Rate loop_rate(500); 104 | sensor_msgs::Imu imu_data; 105 | serial_imu::Imu_0x91_msg imu_0x91_msg; 106 | serial_imu::Imu_0x62_msg imu_0x62_msg; 107 | 108 | while(ros::ok()) 109 | { 110 | size_t num = sp.available(); 111 | if(num!=0) 112 | { 113 | uint8_t buffer[BUF_SIZE]; 114 | 115 | if(num > BUF_SIZE) 116 | num = BUF_SIZE; 117 | 118 | num = sp.read(buffer, num); 119 | if(num > 0) 120 | { 121 | for(int i = 0; i < num; i++) 122 | packet_decode(buffer[i]); 123 | 124 | if (bitmap & 0xff) 125 | { 126 | 127 | imu_data.header.stamp = ros::Time::now(); 128 | imu_data.header.frame_id = "base_link"; 129 | 130 | imu_0x91_msg.header.stamp = ros::Time::now(); 131 | imu_0x91_msg.header.frame_id = "base_0x91_link"; 132 | 133 | imu_0x62_msg.header.stamp = ros::Time::now(); 134 | imu_0x62_msg.header.frame_id = "base_0x62_link"; 135 | 136 | if(id0x62.tag != KItemGWSOL) 137 | { 138 | publish_0x91_data(&id0x91, &imu_0x91_msg); 139 | Imu_0x91_pub.publish(imu_0x91_msg); 140 | 141 | publish_imu_data(&id0x91, &imu_data); 142 | IMU_pub.publish(imu_data); 143 | } 144 | else 145 | { 146 | 147 | publish_0x62_data(&id0x62, &imu_0x62_msg); 148 | 149 | Imu_0x62_pub.publish(imu_0x62_msg); 150 | 151 | 152 | } 153 | } 154 | } 155 | } 156 | loop_rate.sleep(); 157 | } 158 | 159 | sp.close(); 160 | 161 | return 0; 162 | } 163 | 164 | 165 | //void publish_0x91_data(id0x91_t *data, serial_imu::Imu_0x91_msg *data_imu) 166 | void memcpy_imu_data_package(id0x91_t *data, serial_imu::Imu_data_package *data_imu) 167 | { 168 | data_imu->tag = data->tag; 169 | data_imu->bitmap = bitmap; 170 | if(bitmap & BIT_VALID_ID) 171 | data_imu->id = data->id; 172 | 173 | if(bitmap & BIT_VALID_TIME) 174 | data_imu->time = data->time; 175 | 176 | data_imu->frame_rate = frame_rate; 177 | 178 | //data_imu->frame_rate = crc_error_count; 179 | if(bitmap & BIT_VALID_ACC) 180 | { 181 | data_imu->acc_x = data->acc[0]; 182 | data_imu->acc_y = data->acc[1]; 183 | data_imu->acc_z = data->acc[2]; 184 | } 185 | 186 | if(bitmap & BIT_VALID_GYR) 187 | { 188 | data_imu->gyr_x = data->gyr[0]; 189 | data_imu->gyr_y = data->gyr[1]; 190 | data_imu->gyr_z = data->gyr[2]; 191 | } 192 | 193 | if(bitmap & BIT_VALID_MAG) 194 | { 195 | data_imu->mag_x = data->mag[0]; 196 | data_imu->mag_y = data->mag[1]; 197 | data_imu->mag_z = data->mag[2]; 198 | } 199 | 200 | if(bitmap & BIT_VALID_EUL) 201 | { 202 | data_imu->eul_r = data->eul[0]; 203 | data_imu->eul_p = data->eul[1]; 204 | data_imu->eul_y = data->eul[2]; 205 | } 206 | 207 | if(bitmap & BIT_VALID_QUAT) 208 | { 209 | data_imu->quat_w = data->quat[0]; 210 | data_imu->quat_x = data->quat[1]; 211 | data_imu->quat_y = data->quat[2]; 212 | data_imu->quat_z = data->quat[3]; 213 | } 214 | } 215 | 216 | void publish_imu_data(id0x91_t *data, sensor_msgs::Imu *imu_data) 217 | { 218 | imu_data->orientation.x = data->quat[1]; 219 | imu_data->orientation.y = data->quat[2]; 220 | imu_data->orientation.z = data->quat[3]; 221 | imu_data->orientation.w = data->quat[0]; 222 | imu_data->angular_velocity.x = data->gyr[0] * DEG_TO_RAD; 223 | imu_data->angular_velocity.y = data->gyr[1] * DEG_TO_RAD; 224 | imu_data->angular_velocity.z = data->gyr[2] * DEG_TO_RAD; 225 | imu_data->linear_acceleration.x = data->acc[0] * GRA_ACC; 226 | imu_data->linear_acceleration.y = data->acc[1] * GRA_ACC; 227 | imu_data->linear_acceleration.z = data->acc[2] * GRA_ACC; 228 | } 229 | 230 | void publish_0x91_data(id0x91_t *data, serial_imu::Imu_0x91_msg *data_imu) 231 | { 232 | memcpy_imu_data_package(data,&(data_imu->imu_data)); 233 | } 234 | 235 | void publish_0x62_data(id0x62_t *data, serial_imu::Imu_0x62_msg *data_imu) 236 | { 237 | /* */ 238 | data_imu->tag = data->tag; 239 | data_imu->gw_id = data->gw_id; 240 | data_imu->node_num = data->n; 241 | 242 | for (int i = 0; i < data_imu->node_num; i++) 243 | memcpy_imu_data_package(&(data->id0x91[i]), &(data_imu->node_data[i])); 244 | 245 | 246 | } 247 | --------------------------------------------------------------------------------