├── docs ├── readme.md ├── corenav7.gif └── framework.png ├── imu_filter ├── README.md ├── launch │ └── imu_filter.launch ├── package.xml └── src │ └── imu_filter.cpp ├── core_navigation ├── script │ ├── utils │ │ ├── __init__.py │ │ ├── data.pyc │ │ ├── __init__.pyc │ │ ├── __pycache__ │ │ │ ├── data.cpython-35.pyc │ │ │ ├── data.cpython-37.pyc │ │ │ ├── __init__.cpython-35.pyc │ │ │ └── __init__.cpython-37.pyc │ │ ├── data.py │ │ ├── plot.py │ │ └── utils.py │ ├── run_cn_frombag.sh │ └── gp_slip_node.py ├── msg │ ├── GP_Output.msg │ └── GP_Input.msg ├── srv │ └── SetStopping.srv ├── config │ ├── custom_rosconsole.conf │ ├── init_params.yaml │ └── parameters.yaml ├── include │ └── core_navigation │ │ ├── InsConst.h │ │ └── InsUtils.h ├── launch │ └── corenav.launch ├── CMakeLists.txt ├── package.xml └── src │ └── core_nav.cpp ├── gps_core_navigation ├── msg │ ├── GP_Output.msg │ └── GP_Input.msg ├── config │ ├── init_params.yaml │ └── parameters.yaml ├── include │ └── gps_core_navigation │ │ ├── InsConst.h │ │ └── InsUtils.h ├── CMakeLists.txt ├── src │ └── gps_core_nav.cpp ├── launch │ └── gps_corenav.launch └── package.xml ├── VIO Analysis ├── docs │ ├── scene1.PNG │ ├── scenario1.PNG │ ├── scenario2.PNG │ ├── scenario3.PNG │ ├── scenario4.PNG │ └── scenario5.PNG └── README.md ├── Kernel Selection ├── docs │ ├── ours.jpg │ └── allkernels.jpg └── README.md ├── hw_interface_plugins ├── hw_interface_plugin_roboteq │ ├── msg │ │ ├── ActuatorOut.msg │ │ └── RoboteqData.msg │ ├── launch │ │ ├── Left_Roboteq_Interface.launch │ │ ├── Right_Roboteq_Interface.launch │ │ └── launch_params │ │ │ ├── Left_Roboteq_launch_params.yaml │ │ │ └── Right_Roboteq_launch_params.yaml │ ├── roboteq_plugins.xml │ ├── include │ │ └── hw_interface_plugin_roboteq │ │ │ ├── hw_interface_plugin_roboteq_brushed.hpp │ │ │ └── hw_interface_plugin_roboteq.hpp │ ├── package.xml │ └── src │ │ ├── hw_interface_plugin_roboteq_brushed.cpp │ │ └── hw_interface_plugin_roboteq_brushless.cpp └── hw_interface_plugin_adis_imu │ ├── launch │ ├── Adis_Imu_Serial_Interface.launch │ └── launch_params │ │ └── Adis_Imu_Serial_launch_params.yaml │ ├── readme.md │ ├── adis_imu_plugin.xml │ ├── include │ └── hw_interface_plugin_adis_imu │ │ └── hw_interface_plugin_adis_imu_serial.hpp │ └── package.xml ├── pathfinder_control ├── launch │ ├── drive_stop.launch │ ├── control.launch │ ├── teleop.launch │ └── control_sim.launch ├── config │ ├── teleop.yaml │ ├── twist_mux.yaml │ └── pathfinder_simple_sim_control.yaml ├── src │ ├── pathfinder_sim_localization_node.cpp │ └── drive_straight_with_stop.cpp └── package.xml ├── init_core_navigation ├── config │ └── parameters.yaml ├── launch │ └── init_corenav.launch ├── src │ └── init_core_nav.cpp ├── CMakeLists.txt ├── package.xml └── include │ └── init_core_navigation │ └── InitCoreNav.h ├── parameter_utils ├── CMakeLists.txt ├── package.xml └── include │ └── parameter_utils │ └── ParameterUtils.h ├── gp_predictor ├── launch │ └── gp_predictor.launch ├── include │ └── gp_predictor │ │ └── gp_predictor.h └── package.xml ├── hw_interface ├── launch │ ├── hw_interface.launch │ ├── hw_interface_debug.launch │ └── hw_interface_plugins.launch ├── src │ ├── hw_interface_node.cpp │ └── base_UDP_interface.cpp ├── package.xml └── include │ └── hw_interface │ ├── bit_utils.h │ ├── hw_interface.hpp │ ├── base_UDP_interface.hpp │ ├── telemetry.hpp │ └── shared_const_buffer.hpp ├── novatel_launch ├── launch │ └── novatel.launch └── package.xml ├── geometry_utils ├── package.xml ├── tests │ ├── test_so3error.cc │ └── test_math.cc ├── CMakeLists.txt ├── include │ └── geometry_utils │ │ ├── RotationNBase.h │ │ ├── Vector4.h │ │ ├── Vector2.h │ │ ├── Vector3.h │ │ ├── MatrixNxNBase.h │ │ ├── GeometryUtilsSerialization.h │ │ ├── Rotation2.h │ │ ├── YAMLLoader.h │ │ ├── Quaternion.h │ │ ├── Matrix2x2.h │ │ ├── Transform2.h │ │ ├── GeometryUtilsMath.h │ │ ├── Transform3.h │ │ ├── GeometryUtils.h │ │ ├── Matrix3x3.h │ │ └── GeometryUtilsROS.h └── cmake │ └── FindEigen3.cmake ├── LICENSE ├── gps_data_log ├── scripts │ └── gps_log_node.py └── package.xml └── README.md /docs/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /imu_filter/README.md: -------------------------------------------------------------------------------- 1 | # imu_filter 2 | -------------------------------------------------------------------------------- /core_navigation/script/utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /core_navigation/msg/GP_Output.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] mean 3 | float64[] sigma 4 | -------------------------------------------------------------------------------- /docs/corenav7.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/docs/corenav7.gif -------------------------------------------------------------------------------- /docs/framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/docs/framework.png -------------------------------------------------------------------------------- /gps_core_navigation/msg/GP_Output.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] mean 3 | float64[] sigma 4 | -------------------------------------------------------------------------------- /core_navigation/msg/GP_Input.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] time_array 3 | float64[] slip_array 4 | -------------------------------------------------------------------------------- /gps_core_navigation/msg/GP_Input.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] time_array 3 | float64[] slip_array 4 | -------------------------------------------------------------------------------- /VIO Analysis/docs/scene1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scene1.PNG -------------------------------------------------------------------------------- /Kernel Selection/docs/ours.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/Kernel Selection/docs/ours.jpg -------------------------------------------------------------------------------- /VIO Analysis/docs/scenario1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scenario1.PNG -------------------------------------------------------------------------------- /VIO Analysis/docs/scenario2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scenario2.PNG -------------------------------------------------------------------------------- /VIO Analysis/docs/scenario3.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scenario3.PNG -------------------------------------------------------------------------------- /VIO Analysis/docs/scenario4.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scenario4.PNG -------------------------------------------------------------------------------- /VIO Analysis/docs/scenario5.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/VIO Analysis/docs/scenario5.PNG -------------------------------------------------------------------------------- /Kernel Selection/docs/allkernels.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/Kernel Selection/docs/allkernels.jpg -------------------------------------------------------------------------------- /core_navigation/script/utils/data.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/data.pyc -------------------------------------------------------------------------------- /core_navigation/script/utils/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/__init__.pyc -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/msg/ActuatorOut.msg: -------------------------------------------------------------------------------- 1 | int16 fl_speed_cmd 2 | int16 fr_speed_cmd 3 | 4 | int16 bl_speed_cmd 5 | int16 br_speed_cmd 6 | -------------------------------------------------------------------------------- /core_navigation/script/utils/__pycache__/data.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/__pycache__/data.cpython-35.pyc -------------------------------------------------------------------------------- /core_navigation/script/utils/__pycache__/data.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/__pycache__/data.cpython-37.pyc -------------------------------------------------------------------------------- /core_navigation/script/utils/__pycache__/__init__.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/__pycache__/__init__.cpython-35.pyc -------------------------------------------------------------------------------- /core_navigation/script/utils/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav-GP/HEAD/core_navigation/script/utils/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /core_navigation/srv/SetStopping.srv: -------------------------------------------------------------------------------- 1 | bool stopping 2 | --- 3 | float64[225] PvecData 4 | float64[225] QvecData 5 | float64[225] STMvecData 6 | float64[60] HvecData 7 | geometry_msgs/Point PosData 8 | -------------------------------------------------------------------------------- /pathfinder_control/launch/drive_stop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /pathfinder_control/config/teleop.yaml: -------------------------------------------------------------------------------- 1 | teleop_twist_joy: 2 | axis_linear: 1 3 | scale_linear: 0.4 4 | scale_linear_turbo: 1.0 5 | axis_angular: 0 6 | scale_angular: 0.6 7 | scale_angular_turbo: 1.0 8 | enable_button: 0 9 | enable_turbo_button: 2 10 | joy_node: 11 | deadzone: 0.1 12 | autorepeat_rate: 20 13 | -------------------------------------------------------------------------------- /core_navigation/config/custom_rosconsole.conf: -------------------------------------------------------------------------------- 1 | # config/custom_rosconsole.conf 2 | # You can define your own by e.g. copying this file and setting 3 | # ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file 4 | # 5 | log4j.logger.ros=INFO 6 | log4j.logger.ros.core_nav=DEBUG 7 | log4j.logger.ros.roscpp.superdebug=WARN 8 | -------------------------------------------------------------------------------- /pathfinder_control/config/twist_mux.yaml: -------------------------------------------------------------------------------- 1 | topics: 2 | - name : joy 3 | topic : joy_teleop/cmd_vel 4 | timeout : 0.5 5 | priority: 10 6 | - name : external 7 | topic : cmd_vel 8 | timeout : 0.5 9 | priority: 100 10 | locks: 11 | - name : e_stop 12 | topic : e_stop 13 | timeout : 0.0 14 | priority: 255 15 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/launch/Left_Roboteq_Interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/launch/Right_Roboteq_Interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /init_core_navigation/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | imu: 2 | publish_hz: 250 3 | sensor_pub_rate: 250 4 | topic: /imu/dataAdis 5 | 6 | gps_llh: 7 | topic: /bestpos 8 | gps_ecef: 9 | topic: /bestxyz 10 | 11 | frames: 12 | frame_id_out: map 13 | frame_id_fixed: odom 14 | frame_id_imu: imu_link 15 | frame_id_odo: odom_link 16 | -------------------------------------------------------------------------------- /parameter_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(parameter_utils) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | ) 9 | 10 | install(DIRECTORY include/${PROJECT_NAME}/ 11 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 12 | FILES_MATCHING PATTERN "*.h" 13 | ) 14 | -------------------------------------------------------------------------------- /gp_predictor/launch/gp_predictor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /gps_core_navigation/config/init_params.yaml: -------------------------------------------------------------------------------- 1 | bias_a: 2 | x: 0.049725549309 3 | y: 0.280722422325 4 | z: -9.806383392926 5 | bias_g: 6 | x: -0.000007296495 7 | y: -0.000147732137 8 | z: 0.000591663048 9 | init_ecef: 10 | x: 859207.651300000027 11 | y: -4836200.229899999686 12 | z: 4055498.126499999780 13 | init_llh: 14 | x: 0.693481702495 15 | y: -1.394969205710 16 | z: 373.843300000000 17 | -------------------------------------------------------------------------------- /core_navigation/config/init_params.yaml: -------------------------------------------------------------------------------- 1 | mean_bias_a: 2 | x: 0.269614133542350 3 | y: -0.262166124574161 4 | z: -9.804005623794065 5 | mean_bias_g: 6 | x: 0.000268715437386711 7 | y: -0.000161452230267971 8 | z: 0.000678693501054130 9 | init_ecef: 10 | x: 859153.015300000 11 | y: -4836303.72660000 12 | z: 4055378.50100000 13 | init_llh: 14 | x: 0.693457963620326 15 | y: -1.39498384275845 16 | z: 334.993517334743 17 | init_att: 18 | x: 0.0 19 | y: 0.0 20 | z: -0.733038286 21 | -------------------------------------------------------------------------------- /init_core_navigation/launch/init_corenav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /hw_interface/launch/hw_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 18 | 19 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/launch/Adis_Imu_Serial_Interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /hw_interface/launch/hw_interface_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /parameter_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | parameter_utils 4 | 0.0.0 5 | The parameter_utils package 6 | 7 | Nathan Michael 8 | Erik Nelson 9 | Erik Nelson 10 | 11 | GPLv2 12 | 13 | catkin 14 | roscpp 15 | 16 | -------------------------------------------------------------------------------- /imu_filter/launch/imu_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /novatel_launch/launch/novatel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | verbose: false 8 | connection_type: serial 9 | device: /dev/serial/by-path/pci-0000:00:14.0-usb-0:3:1.0-port0 10 | frame_id: /gps 11 | publish_novatel_positions: true 12 | polling_period: 0.1 13 | publish_range_messages: true 14 | publish_novatel_xyz_positions: true 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/readme.md: -------------------------------------------------------------------------------- 1 | Setup 2 | plugin_names: 3 | { 4 | adis_imu_serial : hw_interface_plugin_adis_imu, 5 | } 6 | 7 | adis_imu_serial: 8 | { 9 | adis_imu : "adis_imu", 10 | } 11 | 12 | adis_imu: 13 | { 14 | deviceName : "/dev/serial/by-path/pci-0000:00:14.0-usb-0:1.1:1.0-port0", 15 | baudrate : 115200, 16 | subscribeToTopic : "/example/subscribe", 17 | publishToTopic : "/imu/data", 18 | publishToTopicAdis : "/imu/dataAdis", 19 | } 20 | 21 | -------------------------------------------------------------------------------- /init_core_navigation/src/init_core_nav.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv){ 6 | ros::init(argc, argv, "inertial propagation"); 7 | ros::NodeHandle n("~"); 8 | 9 | InitCoreNav InitCoreNav; 10 | if(!InitCoreNav.Initialize(n)) { 11 | ROS_ERROR("%s: Failed to initialize the nav. filter.", 12 | ros::this_node::getName().c_str()); 13 | return EXIT_FAILURE; 14 | } 15 | ros::spin(); 16 | 17 | return EXIT_SUCCESS; 18 | } 19 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/roboteq_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Roboteq Brushed DC Controller Serial Interface 4 | 5 | 6 | 7 | 8 | 9 | Roboteq Brushless DC Controller Serial Interface 10 | 11 | 12 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/launch/launch_params/Adis_Imu_Serial_launch_params.yaml: -------------------------------------------------------------------------------- 1 | plugin_names: 2 | { 3 | adis_imu_serial : hw_interface_plugin_adis_imu, 4 | } 5 | 6 | adis_imu_serial: 7 | { 8 | adis_imu : "adis_imu", 9 | } 10 | 11 | adis_imu: 12 | { 13 | deviceName : "/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0", 14 | baudrate : 115200, 15 | subscribeToTopic : "/example/subscribe", 16 | publishToTopic : "/imu/data", 17 | publishToTopicAdis : "/imu/dataAdis", 18 | } 19 | 20 | -------------------------------------------------------------------------------- /hw_interface/launch/hw_interface_plugins.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /core_navigation/script/run_cn_frombag.sh: -------------------------------------------------------------------------------- 1 | !/bin/bash 2 | SESSION=core_nav 3 | ## Start up 4 | tmux -2 new-session -d -s $SESSION 5 | 6 | tmux split-window -v -p 50 7 | tmux split-window -h -p 50 8 | tmux select-pane -t 0 9 | tmux split-window -h -p 50 10 | 11 | 12 | tmux send-keys -t 0 "sleep 3; roslaunch init_core_nav init_corenav.launch " C-m 13 | tmux send-keys -t 1 "sleep 5; roslaunch core_nav corenav.launch" C-m 14 | tmux send-keys -t 2 "sleep 4; cd ~/Desktop; rosbag play 2019-06-28-17-16-48.bag " C-m 15 | tmux send-keys -t 3 "cd ../../..; source devel/setup.bash" C-m 16 | tmux send-keys -t 4 "rqt_multiplot" C-m 17 | 18 | # Attach to session 19 | tmux -2 attach-session -t $SESSION 20 | -------------------------------------------------------------------------------- /geometry_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | geometry_utils 4 | 0.0.9 5 | geometry_utils 6 | 7 | Nathan Michael 8 | Erik Nelson 9 | Erik Nelson 10 | 11 | GPLv2 12 | 13 | catkin 14 | roscpp 15 | geometry_msgs 16 | tf2 17 | 18 | tf2 19 | roscpp 20 | 21 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/launch/launch_params/Left_Roboteq_launch_params.yaml: -------------------------------------------------------------------------------- 1 | plugin_names: 2 | { 3 | brushed : hw_interface_plugin_roboteq 4 | } 5 | 6 | brushed: 7 | { 8 | Left_Roboteq : Left_Roboteq, 9 | } 10 | 11 | Right_Roboteq: 12 | { 13 | deviceName : "/dev/serial/by-path/pci-0000:00:14.0-usb-0:5.3.3:1.0-port1", 14 | baudrate : 115200, 15 | subscribeToTopic : "/control/actuatorout", 16 | publishToTopic : "/roboteq/drivemotorin/left", 17 | controllerType : "Left_Drive", 18 | initializationCmd : "encoder_counter_absolute feedback motor_amps battery_amps", 19 | initCmdCycle : 20, 20 | wheelType : "", 21 | } 22 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/launch/launch_params/Right_Roboteq_launch_params.yaml: -------------------------------------------------------------------------------- 1 | plugin_names: 2 | { 3 | brushed : hw_interface_plugin_roboteq 4 | } 5 | 6 | brushed: 7 | { 8 | Right_Roboteq : Right_Roboteq, 9 | } 10 | 11 | Left_Roboteq: 12 | { 13 | deviceName : "/dev/serial/by-path/pci-0000:00:14.0-usb-0:5.3.3:1.0-port0", 14 | baudrate : 115200, 15 | subscribeToTopic : "/control/actuatorout", 16 | publishToTopic : "/roboteq/drivemotorin/right", 17 | controllerType : "Right_Drive", 18 | initializationCmd : "encoder_counter_absolute feedback motor_amps battery_amps", 19 | initCmdCycle : 20, 20 | wheelType : "", 21 | } 22 | -------------------------------------------------------------------------------- /core_navigation/include/core_navigation/InsConst.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsConst.h 3 | * @brief House values that are repeatedly used 4 | * @author Cagri 5 | */ 6 | 7 | #pragma once 8 | 9 | namespace INS { 10 | 11 | const double omega_ie = 7.292115e-5; // rotation of Earth in rad/sec 12 | const double Ro = 6378137.0; //WGS84 Equatorial Radius 13 | const double Rp = 6356752.31425; //WGS84 Polar Radius 14 | const double flat= 1.0/298.257223563; // WGS84 Earth flattening 15 | const double ecc = 0.0818191909425; // WGS84 Eccentricity 0.0818191908426 16 | const double t_const = pow((1.0 - flat),2.0); // constant 17 | const double wheel_radius=0.11; //meters 18 | const double wheelbase=0.555; //meters 19 | const double gravity= 9.80665; 20 | const double PI = 3.14159265358979; 21 | 22 | } 23 | -------------------------------------------------------------------------------- /gps_core_navigation/include/gps_core_navigation/InsConst.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsConst.h 3 | * @brief House values that are repeatedly used 4 | * @author Cagri 5 | */ 6 | 7 | #pragma once 8 | 9 | namespace INS { 10 | 11 | const double omega_ie = 7.292115e-5; // rotation of Earth in rad/sec 12 | const double Ro = 6378137.0; //WGS84 Equatorial Radius 13 | const double Rp = 6356752.31425; //WGS84 Polar Radius 14 | const double flat= 1.0/298.257223563; // WGS84 Earth flattening 15 | const double ecc = 0.0818191909425; // WGS84 Eccentricity 0.0818191908426 16 | const double t_const = pow((1.0 - flat),2.0); // constant 17 | const double wheel_radius=0.11; //meters TODO ~0.12 18 | const double wheelbase=0.5; //meters TODO 19 | const double gravity= 9.80665; 20 | const double PI = 3.14159265358979; 21 | 22 | } 23 | -------------------------------------------------------------------------------- /core_navigation/launch/corenav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | 13 | position: {x: 0.691949695095547, y: -1.395750757746275, z: 312.2643980319930} 14 | velocity: {vx: 0.0, vy: 0.0, vz: 0.0} 15 | orientation: {qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /pathfinder_control/config/pathfinder_simple_sim_control.yaml: -------------------------------------------------------------------------------- 1 | pathfinder: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | front_left_velocity_controller: 7 | type: velocity_controllers/JointVelocityController 8 | joint: base_to_front_left 9 | pid: {p: 1, i: 0, d: 0} 10 | back_left_velocity_controller: 11 | type: velocity_controllers/JointVelocityController 12 | joint: base_to_back_left 13 | pid: {p: 1, i: 0, d: 0} 14 | front_right_velocity_controller: 15 | type: velocity_controllers/JointVelocityController 16 | joint: base_to_front_right 17 | pid: {p: 1, i: 0, d: 0} 18 | back_right_velocity_controller: 19 | type: velocity_controllers/JointVelocityController 20 | joint: base_to_back_right 21 | pid: {p: 1, i: 0, d: 0} 22 | 23 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/adis_imu_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | This is a serial interface for the ADIS IMU system 16 | 17 | 18 | -------------------------------------------------------------------------------- /core_navigation/script/utils/data.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | 3 | 4 | def load_co2(file_path): 5 | co2 = pd.read_csv(file_path, header=None, index_col=None) 6 | 7 | co2_x = co2[1].values 8 | co2_y = co2[0].values 9 | 10 | return co2_x, co2_y 11 | 12 | 13 | def load_erie(file_path): 14 | erie = pd.read_csv(file_path, header=0, names=["date", "level"], index_col=None) 15 | 16 | erie = erie["level"].values 17 | erie = erie[:len(erie)-1] 18 | return erie 19 | 20 | def load_slip(file_path): 21 | slip = pd.read_csv(file_path, header=0, names=["date", "level"], index_col=None) 22 | 23 | slip = slip["level"].values 24 | slip = slip[:len(slip)-1] 25 | return slip 26 | 27 | def load_airline(file_path): 28 | airline = pd.read_csv(file_path, header=None) 29 | airline = airline[1].values 30 | 31 | return airline 32 | 33 | 34 | def load_solar(file_path): 35 | solar = pd.read_csv(file_path, sep=' ', header=None, skiprows=7) 36 | solar = solar[1].values 37 | 38 | return solar 39 | -------------------------------------------------------------------------------- /init_core_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(init_core_nav) 3 | 4 | set (CMAKE_CXX_STANDARD 11) 5 | 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | geometry_utils 10 | parameter_utils 11 | geometry_msgs 12 | sensor_msgs 13 | tf2_ros 14 | tf 15 | roslib 16 | novatel_gps_msgs 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS include 21 | LIBRARIES ${PROJECT_NAME} 22 | CATKIN_DEPENDS 23 | roscpp 24 | geometry_utils 25 | parameter_utils 26 | geometry_msgs 27 | sensor_msgs 28 | tf2_ros 29 | tf 30 | roslib 31 | novatel_gps_msgs 32 | ) 33 | 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | link_directories( 40 | ${catkin_LIBRARY_DIRS} 41 | ) 42 | 43 | add_library(${PROJECT_NAME} src/InitCoreNav.cpp) 44 | target_link_libraries(${PROJECT_NAME} 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp) 49 | target_link_libraries(${PROJECT_NAME}_node 50 | ${PROJECT_NAME} 51 | ${catkin_LIBRARIES} 52 | ) 53 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/msg/RoboteqData.msg: -------------------------------------------------------------------------------- 1 | # A 2 | int16[] motor_amps 3 | 4 | # AI 5 | int16[] analog_inputs 6 | 7 | # AIC 8 | int16[] analog_inputs_conversion 9 | 10 | # BA 11 | int16[] battery_amps 12 | 13 | # C - returns absolute encoder value; 32bit range +/-2147483648 counts 14 | int32[] encoder_counter_absolute 15 | 16 | # CR - returns amt of counts measured from last query (small numbers usually returned) 17 | int32[] encoder_count_relative 18 | 19 | # BS 20 | int16[] bl_motor_speed_rpm 21 | 22 | # DI 23 | bool[] individual_digital_inputs 24 | 25 | # DR 26 | uint8[] destination_reached 27 | 28 | # FF 29 | uint8 fault_flags 30 | # Where 31 | # f1 = Overheat 32 | # f2 = Overvoltage 33 | # f3 = Undervoltage 34 | # f4 = Short circuit 35 | # f5 = Emergency stop 36 | # f6 = Brushless sensor fault 37 | # f7 = MOSFET failure 38 | # f8 = Default configuration loaded at startup 39 | 40 | # F 41 | int16[] feedback 42 | 43 | # VAR 44 | int32[] user_integer_variable 45 | 46 | # V where 47 | # volts[0] - internal volts 48 | # volts[1] - battery volts 49 | # volts[2] - 5V output 50 | uint16[] volts 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Cagri Kilic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /pathfinder_control/src/pathfinder_sim_localization_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class SimLocalization 6 | { 7 | public: 8 | // Members 9 | ros::NodeHandle nh; 10 | ros::Subscriber odom_sub; 11 | tf::TransformBroadcaster tf_broad; 12 | geometry_msgs::TransformStamped odom_trans; 13 | nav_msgs::Odometry odom; 14 | // Methods 15 | SimLocalization() 16 | { 17 | odom_sub = nh.subscribe("odom",1,&SimLocalization::odomCallback,this); 18 | } 19 | void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) 20 | { 21 | odom_trans.header = msg->header; 22 | odom_trans.child_frame_id = msg->child_frame_id; 23 | odom_trans.transform.translation.x = msg->pose.pose.position.x; 24 | odom_trans.transform.translation.y = msg->pose.pose.position.y; 25 | odom_trans.transform.translation.z = msg->pose.pose.position.z; 26 | odom_trans.transform.rotation = msg->pose.pose.orientation; 27 | tf_broad.sendTransform(odom_trans); 28 | } 29 | }; 30 | 31 | int main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "pathfinder_sim_localization"); 34 | SimLocalization simLocalization; 35 | ros::spin(); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /gps_core_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gps_core_nav) 3 | 4 | set (CMAKE_CXX_STANDARD 11) 5 | 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | hw_interface_plugin_roboteq 9 | roscpp 10 | geometry_utils 11 | parameter_utils 12 | geometry_msgs 13 | sensor_msgs 14 | tf2_ros 15 | tf 16 | std_msgs 17 | message_generation 18 | novatel_gps_msgs 19 | ) 20 | 21 | #add_message_files( 22 | # FILES 23 | # GP_Input.msg 24 | # GP_Output.msg 25 | #) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | ) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | LIBRARIES ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | roscpp 37 | geometry_utils 38 | parameter_utils 39 | geometry_msgs 40 | sensor_msgs 41 | tf2_ros 42 | tf 43 | novatel_gps_msgs 44 | ) 45 | 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ) 50 | 51 | link_directories( 52 | ${catkin_LIBRARY_DIRS} 53 | ) 54 | 55 | add_library(${PROJECT_NAME} src/gps_CoreNav.cpp) 56 | target_link_libraries(${PROJECT_NAME} 57 | ${catkin_LIBRARIES} 58 | ) 59 | 60 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp) 61 | target_link_libraries(${PROJECT_NAME}_node 62 | ${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | ) 65 | -------------------------------------------------------------------------------- /core_navigation/include/core_navigation/InsUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsUtils.h 3 | * @brief Tools required to process inertial data 4 | * @author Cagri, Ryan 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace INS { 25 | 26 | typedef Eigen::VectorXd Vector; 27 | typedef Eigen::MatrixXd Matrix; 28 | 29 | typedef Eigen::Matrix Vector3; 30 | typedef Eigen::Matrix Vector6; 31 | typedef Eigen::Matrix Matrix3; 32 | 33 | Vector3 calc_gravity(const double latitude, const double height); 34 | 35 | Matrix3 skew_symm(const Vector3 vec); 36 | 37 | Matrix3 eul_to_dcm(double phi, double theta, double psi); 38 | 39 | Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, Vector3 insLLHPlus, Vector3 insVelPlus, double dt, Matrix3 CbnPlus, double omega_ie,Vector3 omega_n_in,Vector3 f_ib_b,double gravity); 40 | 41 | Matrix calc_Q(double R_N, double R_E, Vector3 insLLHPlus, double dt, Matrix3 CbnPlus,Vector3 f_ib_b); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /init_core_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | init_core_nav 4 | 0.1.0 5 | Init Core Navigation 6 | 7 | Cagri Kilic 8 | Cagri Kilic 9 | 10 | GPLv2 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_utils 16 | parameter_utils 17 | geometry_msgs 18 | sensor_msgs 19 | nav_msgs 20 | tf2_ros 21 | tf 22 | roslib 23 | nav_msgs 24 | novatel_gps_msgs 25 | 26 | roscpp 27 | geometry_utils 28 | parameter_utils 29 | geometry_msgs 30 | sensor_msgs 31 | roslib 32 | tf2_ros 33 | tf 34 | novatel_gps_msgs 35 | 36 | 37 | -------------------------------------------------------------------------------- /gps_core_navigation/include/gps_core_navigation/InsUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsUtils.h 3 | * @brief Tools required to process inertial data 4 | * @author Cagri, Ryan 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace INS { 25 | 26 | typedef Eigen::VectorXd Vector; 27 | typedef Eigen::MatrixXd Matrix; 28 | 29 | typedef Eigen::Matrix Vector3; 30 | typedef Eigen::Matrix Vector6; 31 | typedef Eigen::Matrix Matrix3; 32 | 33 | Vector3 calc_gravity(const double latitude, const double height); 34 | 35 | Matrix3 skew_symm(const Vector3 vec); 36 | 37 | Matrix3 eul_to_dcm(double phi, double theta, double psi); 38 | 39 | Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, Vector3 insLLHPlus, Vector3 insVelPlus, double dt, Matrix3 CbnPlus, double omega_ie,Vector3 omega_n_in,Vector3 f_ib_b,double gravity); 40 | 41 | Matrix calc_Q(double R_N, double R_E, Vector3 insLLHPlus, double dt, Matrix3 CbnPlus,Vector3 f_ib_b); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /core_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(core_nav) 3 | 4 | set (CMAKE_CXX_STANDARD 11) 5 | 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | hw_interface_plugin_roboteq 9 | roscpp 10 | geometry_utils 11 | parameter_utils 12 | geometry_msgs 13 | sensor_msgs 14 | tf2_ros 15 | tf 16 | std_msgs 17 | message_generation 18 | ) 19 | 20 | add_message_files( 21 | FILES 22 | GP_Input.msg 23 | GP_Output.msg 24 | ) 25 | 26 | add_service_files( 27 | FILES 28 | SetStopping.srv 29 | ) 30 | 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | geometry_msgs 36 | ) 37 | 38 | catkin_package( 39 | INCLUDE_DIRS include 40 | LIBRARIES ${PROJECT_NAME} 41 | CATKIN_DEPENDS 42 | roscpp 43 | geometry_utils 44 | parameter_utils 45 | geometry_msgs 46 | sensor_msgs 47 | tf2_ros 48 | tf 49 | ) 50 | 51 | include_directories( 52 | include 53 | ${catkin_INCLUDE_DIRS} 54 | ) 55 | 56 | link_directories( 57 | ${catkin_LIBRARY_DIRS} 58 | ) 59 | 60 | add_library(${PROJECT_NAME} src/CoreNav.cpp) 61 | target_link_libraries(${PROJECT_NAME} 62 | ${catkin_LIBRARIES} 63 | ) 64 | 65 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp) 66 | target_link_libraries(${PROJECT_NAME}_node 67 | ${PROJECT_NAME} 68 | ${catkin_LIBRARIES} 69 | ) 70 | -------------------------------------------------------------------------------- /geometry_utils/tests/test_so3error.cc: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_DYN_LINK 2 | #define BOOST_TEST_MODULE test_equals 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace gu = geometry_utils; 9 | 10 | BOOST_AUTO_TEST_CASE(so3_matrix) { 11 | gu::Rot3 r1(0, 0, 0); 12 | gu::Rot3 r2(0, 0, 0); 13 | gu::Rot3 r3(M_PI/2.0, 0, 0); 14 | 15 | BOOST_CHECK_EQUAL( r1.Error(r2), 0.0 ); 16 | BOOST_CHECK_EQUAL( r1.Error(r3), 1.0 ); 17 | } 18 | 19 | BOOST_AUTO_TEST_CASE(so3_quat) { 20 | gu::Quat q1 = gu::RToQuat(gu::Rot3(0, 0, 0)); 21 | gu::Quat q2 = gu::RToQuat(gu::Rot3(0, 0, 0)); 22 | gu::Quat q3 = gu::RToQuat(gu::Rot3(M_PI/2.0, 0, 0)); 23 | 24 | gu::Rot3 r1(q1); 25 | gu::Rot3 r2(q2); 26 | gu::Rot3 r3(q3); 27 | 28 | gu::Vec3 v1 = r1.Row(2); 29 | gu::Vec3 v2 = r2.Row(2); 30 | gu::Vec3 v3 = r3.Row(2); 31 | 32 | BOOST_CHECK_EQUAL( acos(v1.Dot(v2)), 0.0 ); 33 | BOOST_CHECK_CLOSE( acos(v1.Dot(v3)), M_PI/2.0 , 1e-10); 34 | 35 | BOOST_CHECK_CLOSE( q1.Error(q2).AxisAngle()(0), 0.0, 1e-10); 36 | BOOST_CHECK_CLOSE( q1.Error(q3).AxisAngle()(0), M_PI/2, 1e-10); 37 | 38 | BOOST_CHECK_CLOSE( acos(v1.Dot(v2)), q1.Error(q2).AxisAngle()(0) , 1e-10); 39 | BOOST_CHECK_CLOSE( acos(v1.Dot(v3)), q1.Error(q3).AxisAngle()(0) , 1e-10); 40 | } 41 | -------------------------------------------------------------------------------- /gps_core_navigation/src/gps_core_nav.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Cagri 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | int main(int argc, char** argv){ 9 | ros::init(argc, argv, "inertial propagation"); 10 | ros::NodeHandle n("~"); 11 | 12 | gps_CoreNav gps_coreNav; 13 | if(!gps_coreNav.Initialize(n)) { 14 | ROS_ERROR("%s: Failed to initialize the nav. filter.", 15 | ros::this_node::getName().c_str()); 16 | return EXIT_FAILURE; 17 | } 18 | while(ros::ok()) 19 | { 20 | if(gps_coreNav.propagate_flag) 21 | { 22 | gps_coreNav.Propagate(gps_coreNav.imu,gps_coreNav.odo,gps_coreNav.cmd,gps_coreNav.encoderLeft,gps_coreNav.encoderRight,gps_coreNav.joint, gps_coreNav.gps_ecef, gps_coreNav.gps_llh); 23 | //ROS_INFO("after Propagate\n"); 24 | gps_coreNav.propagate_flag =false; 25 | } 26 | 27 | if(gps_coreNav.update_flag) 28 | { 29 | gps_coreNav.Update(gps_coreNav.odo,gps_coreNav.joint); 30 | //ROS_INFO("after Propagate\n"); 31 | gps_coreNav.update_flag =false; 32 | } 33 | ros::spinOnce(); 34 | } 35 | //ros::spin(); 36 | 37 | return EXIT_SUCCESS; 38 | } 39 | -------------------------------------------------------------------------------- /core_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | core_nav 4 | 0.1.0 5 | Core Navigation 6 | 7 | Cagri Kilic 8 | Cagri Kilic 9 | 10 | GPLv2 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_utils 16 | parameter_utils 17 | geometry_msgs 18 | sensor_msgs 19 | nav_msgs 20 | tf2_ros 21 | tf 22 | hw_interface_plugin_roboteq 23 | std_msgs 24 | message_generation 25 | roscpp 26 | geometry_utils 27 | parameter_utils 28 | geometry_msgs 29 | sensor_msgs 30 | nav_msgs 31 | tf2_ros 32 | tf 33 | hw_interface_plugin_roboteq 34 | std_msgs 35 | message_runtime 36 | 37 | -------------------------------------------------------------------------------- /gps_core_navigation/launch/gps_corenav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /geometry_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(geometry_utils) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS 11 | include 12 | ${EIGEN3_INCLUDE_DIR} 13 | LIBRARIES 14 | DEPENDS 15 | Eigen3 16 | ) 17 | 18 | include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 19 | 20 | install(DIRECTORY include/${PROJECT_NAME}/ 21 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 22 | FILES_MATCHING PATTERN "*.h" 23 | ) 24 | 25 | install(DIRECTORY cmake/ 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 27 | FILES_MATCHING PATTERN "*.cmake" 28 | ) 29 | 30 | add_executable(test_math tests/test_math.cc) 31 | target_link_libraries(test_math 32 | ${catkin_LIBRARIES} 33 | boost_unit_test_framework 34 | ) 35 | 36 | add_executable(test_base tests/test_base.cc) 37 | target_link_libraries(test_base 38 | ${catkin_LIBRARIES} 39 | boost_unit_test_framework 40 | ) 41 | 42 | add_executable(test_equals tests/test_equals.cc) 43 | target_link_libraries(test_equals 44 | ${catkin_LIBRARIES} 45 | boost_unit_test_framework 46 | ) 47 | 48 | add_executable(test_so3error tests/test_so3error.cc) 49 | target_link_libraries(test_so3error 50 | ${catkin_LIBRARIES} 51 | boost_unit_test_framework 52 | ) 53 | -------------------------------------------------------------------------------- /gps_core_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps_core_nav 4 | 0.1.0 5 | GPS Core Navigation 6 | 7 | Cagri Kilic 8 | Cagri Kilic 9 | 10 | GPLv2 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_utils 16 | parameter_utils 17 | geometry_msgs 18 | sensor_msgs 19 | nav_msgs 20 | tf2_ros 21 | tf 22 | hw_interface_plugin_roboteq 23 | std_msgs 24 | message_generation 25 | novatel_gps_msgs 26 | roscpp 27 | geometry_utils 28 | parameter_utils 29 | geometry_msgs 30 | sensor_msgs 31 | tf2_ros 32 | tf 33 | hw_interface_plugin_roboteq 34 | std_msgs 35 | message_runtime 36 | novatel_gps_msgs 37 | 38 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/RotationNBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_ROTATIONN_H 2 | #define GEOMETRY_UTILS_ROTATIONN_H 3 | 4 | #include 5 | #include 6 | #include "VectorNBase.h" 7 | #include "MatrixNxNBase.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct RotationNBase : MatrixNxNBase { 13 | RotationNBase() : MatrixNxNBase() { this->Eye(); } 14 | 15 | RotationNBase(const RotationNBase& in) : MatrixNxNBase(in.data) {} 16 | RotationNBase(const boost::array& in) : MatrixNxNBase(in) {} 17 | RotationNBase(T (&in)[N * N]) : MatrixNxNBase(in) {} 18 | RotationNBase(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 19 | RotationNBase(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 20 | 21 | virtual inline MatrixNxNBase Inv() const { 22 | return this->Trans(); 23 | } 24 | }; 25 | 26 | template 27 | inline RotationNBase operator*(const float& lhs, 28 | const RotationNBase& rhs) { 29 | return RotationNBase(rhs * lhs); 30 | } 31 | 32 | template 33 | inline RotationNBase operator*(const double& lhs, 34 | const RotationNBase& rhs) { 35 | return RotationNBase(rhs * lhs); 36 | } 37 | 38 | template 39 | inline RotationNBase Inv(const RotationNBase& m) { 40 | return m.Inv(); 41 | } 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector4.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR4_H 2 | #define GEOMETRY_UTILS_VECTOR4_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector4Base : VectorNBase { 10 | Vector4Base() : VectorNBase() {} 11 | Vector4Base(T val) : VectorNBase(val) {} 12 | Vector4Base(const Vector4Base& in) : VectorNBase(in.data) {} 13 | Vector4Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector4Base(T (&in)[4]) : VectorNBase(in) {} 15 | Vector4Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector4Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector4Base(T v1, T v2, T v3, T v4) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | this->data[2] = v3; 22 | this->data[3] = v4; 23 | } 24 | }; 25 | 26 | inline Vector4Base operator*(const float& lhs, 27 | const Vector4Base& rhs) { 28 | return Vector4Base(rhs * lhs); 29 | } 30 | 31 | inline Vector4Base operator*(const double& lhs, 32 | const Vector4Base& rhs) { 33 | return Vector4Base(rhs * lhs); 34 | } 35 | 36 | typedef Vector4Base Vector4f; 37 | typedef Vector4Base Vec4f; 38 | 39 | typedef Vector4Base Vector4d; 40 | typedef Vector4Base Vec4d; 41 | 42 | typedef Vector4Base Vector4; 43 | typedef Vector4Base Vec4; 44 | 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR2_H 2 | #define GEOMETRY_UTILS_VECTOR2_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector2Base : VectorNBase { 10 | Vector2Base() : VectorNBase() {} 11 | Vector2Base(T val) : VectorNBase(val) {} 12 | Vector2Base(const Vector2Base& in) : VectorNBase(in.data) {} 13 | Vector2Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector2Base(T (&in)[2]) : VectorNBase(in) {} 15 | Vector2Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector2Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector2Base(T v1, T v2) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | } 22 | 23 | T X() const { return this->data[0]; } 24 | T Y() const { return this->data[1]; } 25 | }; 26 | 27 | inline Vector2Base operator*(const float& lhs, 28 | const Vector2Base& rhs) { 29 | return Vector2Base(rhs * lhs); 30 | } 31 | 32 | inline Vector2Base operator*(const double& lhs, 33 | const Vector2Base& rhs) { 34 | return Vector2Base(rhs * lhs); 35 | } 36 | 37 | typedef Vector2Base Vector2f; 38 | typedef Vector2Base Vec2f; 39 | 40 | typedef Vector2Base Vector2d; 41 | typedef Vector2Base Vec2d; 42 | 43 | typedef Vector2Base Vector2; 44 | typedef Vector2Base Vec2; 45 | 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /gps_data_log/scripts/gps_log_node.py: -------------------------------------------------------------------------------- 1 | # /* @file gps_log_node.py 2 | # * @brief Novatel GPS Logger 3 | # * @author Nicholas Ohi 4 | # * @author Cagri Kilic 5 | # */ 6 | #!/usr/bin/env python 7 | import rospy 8 | import serial 9 | from datetime import datetime 10 | 11 | def talker(): 12 | ser = serial.Serial('/dev/serial/by-path/pci-0000:00:14.0-usb-0:3:1.0-port0', 115200, timeout=0) 13 | time_now = datetime.now() 14 | time_string = time_now.strftime("%Y-%m-%d-%H-%M-%S") 15 | gps_filename = time_string + "_gps.bin" 16 | timestamp_filename = time_string + "_ros_time.txt" 17 | gps_file = open(gps_filename,"w") 18 | timestamp_file = open(timestamp_filename,"w") 19 | rospy.init_node('gps_log_node', anonymous=False) 20 | rate = rospy.Rate(50) # Hz 21 | time_stamp_delta_time = 0.1 # sec 22 | prev_time = rospy.Time.now().to_sec() - time_stamp_delta_time 23 | rospy.loginfo("GPS Logger Running...") 24 | while not rospy.is_shutdown(): 25 | data = ser.read(1000) 26 | #print(len(data)) 27 | if len(data) > 0: 28 | ros_time = rospy.Time.now().to_sec() 29 | if ros_time - prev_time >= time_stamp_delta_time: 30 | ros_time_string = str(ros_time) + "\n" 31 | #print(ros_time_string) 32 | timestamp_file.write(ros_time_string) 33 | prev_time = ros_time 34 | gps_file.write(data) 35 | rate.sleep() 36 | gps_file.close() 37 | timestamp_file.close() 38 | ser.close() 39 | 40 | if __name__ == '__main__': 41 | try: 42 | talker() 43 | except rospy.ROSInterruptException: 44 | pass 45 | -------------------------------------------------------------------------------- /hw_interface/src/hw_interface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | void mySigintHandler(int sig) 7 | { 8 | // Do some custom action. 9 | // For example, publish a stop message to some other nodes. 10 | 11 | // All the default sigint handler does is call shutdown() 12 | ros::shutdown(); 13 | } 14 | 15 | int main(int argc, char **argv) 16 | { 17 | if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) { 18 | ros::console::notifyLoggerLevelsChanged(); 19 | } 20 | std::string node_type = "hw_interface"; 21 | ROS_INFO("hw_interface Start"); 22 | ros::init(argc, argv, node_type, ros::init_options::NoSigintHandler); 23 | ROS_INFO(" - ros::init complete"); 24 | if(ros::param::get("node_type", node_type)==false) node_type = "hw_interface"; 25 | ros::NodeHandlePtr nh = ros::NodeHandlePtr(new ros::NodeHandle()); 26 | ROS_INFO(" - node handle created"); 27 | 28 | //override default ros handler 29 | signal(SIGINT, mySigintHandler); 30 | 31 | boost::scoped_ptr hwInterfacePtr(new hw_interface(nh)); 32 | ROS_DEBUG("HW_Interface Obj start"); 33 | 34 | while(nh->ok()) 35 | { 36 | //This is where monitoring of the interfaces can happen. 37 | //Monitoring can also be implemented into the ASIO Thread pool by posting 38 | //the monitoring function into the work queue 39 | ros::spin(); 40 | } 41 | 42 | hwInterfacePtr.reset(); 43 | ROS_DEBUG("HW_Interface Closing"); 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /Kernel Selection/README.md: -------------------------------------------------------------------------------- 1 | ## Overview 2 | 3 | **Author: Cagri Kilic
4 | Affiliation: [WVU NAVLAB](https://navigationlab.wvu.edu/)
5 | Maintainer: Cagri Kilic, cakilic@mix.wvu.edu** 6 | 7 | Supplementary Kernel Function Selection analyses for the Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Proprioceptive Localization paper. 8 | 9 | The Kernel Functions are used from GPy Gaussian Process (GP) framework https://sheffieldml.github.io/GPy/ and scikit-learn https://scikit-learn.org/ libraries 10 | 11 | The first figure shows the kernel function(s) representation against the same slip data. The aim is heuristically find the best representation of the slip prediction over time. We assumed that slip could happen in two ways. 1) Randomly due to traversing over a rock, entering a pothole, or small local terrain imperfections, 2) continuously due to the mechanical properties of deformable terrains. These two slip behaviors can be represented as spikes for random slips and slowly increasing slip due to continuous slippage. 12 | 13 | ## Kernel Functions 14 |

15 | architecture 16 |

17 | 18 | After careful consideration through mathematical expressions of the kernels, field tests, and simulation analyses, we have selected two kernel functions: Brownian Kernel to model random, high impulsive slippage, and Radial Basis Function Kernel to model continuous similar slip values. Then we have built the composite kernel model to capture both slippage behaviors during traversal as shown in second figure. 19 | 20 | ## Selected Composite Kernel (RBF * Brownian) 21 |

22 | architecture 23 |

24 | 25 | 26 | -------------------------------------------------------------------------------- /gps_core_navigation/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | init: 2 | position: 3 | # x: 0.69234593 4 | # y: -1.39575202 5 | # z: 312.2643980319930 6 | covx: 1.218469679146834e-14 7 | covy: 1.218469679146834e-14 8 | covz: 0.10000000000000 9 | velocity: 10 | x: 0.0 11 | y: 0.0 12 | z: 0.0 13 | covx: 4.000000000000001e-07 14 | covy: 4.000000000000001e-07 15 | covz: 4.000000000000001e-07 16 | 17 | orientation: 18 | x: 0.0 19 | y: 0.0 20 | z: 4.014 21 | covx: 1.218469679146834e-06 22 | covy: 1.218469679146834e-06 23 | covz: 0.01 24 | # bias: 25 | # accel: 26 | # x: 7.549135895545244e-04 27 | # y: 0.001805886718248 28 | # z: 0.002395586011438 29 | # gyro: 30 | # x: 8.674066138832567e-05 31 | # y: 1.005391303275586e-04 32 | # z: 8.919962404601004e-05 33 | # ecef: 34 | # x: 856206.8996 35 | # y: -4841351.0969 36 | # z: 4049888.7599 37 | 38 | imu: 39 | publish_hz: 250 40 | sensor_pub_rate: 250 41 | topic: /imu/dataAdis 42 | noise: 43 | sig_gyro_inRun : 7.757018897752577e-06 44 | sig_ARW : 2.908882086657216e-05 45 | sig_accel_inRun : 3.139200000000000e-05 46 | sig_VRW : 1.333333333333333e-04 47 | 48 | odo: 49 | topic: /dead_reckoning/odometry 50 | updates: 51 | do_zupt: false 52 | do_zaru: false 53 | 54 | encoderLeft: 55 | topic: /roboteq/drivemotorin/left 56 | 57 | encoderRight: 58 | topic: /roboteq/drivemotorin/right 59 | cmd: 60 | topic: /cmd_vel_out 61 | joint: 62 | topic: /dead_reckoning/joint_states 63 | gps_llh: 64 | topic: /bestpos 65 | gps_ecef: 66 | topic: /bestxyz 67 | # gp: 68 | # topic: /core_nav/core_nav/gp_result 69 | 70 | frames: 71 | frame_id_out: map 72 | frame_id_fixed: odom 73 | frame_id_imu: imu_link 74 | frame_id_odo: odom_link 75 | 76 | wheel: 77 | T_r_: 0.685 78 | s_or_: 0.0 79 | s_delta_or_: 0.0 80 | -------------------------------------------------------------------------------- /gp_predictor/include/gp_predictor/gp_predictor.h: -------------------------------------------------------------------------------- 1 | #ifndef GP_PREDICTOR_H_ 2 | #define GP_PREDICTOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | 8 | #include 9 | #include 10 | #include "std_msgs/Int64.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | class GpPredictor 19 | { 20 | public: 21 | 22 | GpPredictor(ros::NodeHandle &); 23 | 24 | typedef Eigen::Matrix Vector3; 25 | typedef Eigen::MatrixXd Matrix; 26 | 27 | void mobility(bool flag); 28 | void mobilityCallback(const std_msgs::Int64::ConstPtr& msg); 29 | void GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_); 30 | bool LoadParameters(const ros::NodeHandle& nh_); 31 | GpPredictor::Vector3 llh_to_enu(const double latitude, const double longitude, const double height); 32 | 33 | core_nav::GP_Input slip_msg; 34 | core_nav::GP_Output gp_data_; 35 | 36 | Eigen::Matrix R_IP; 37 | Eigen::Matrix R_IP_1; 38 | Eigen::Matrix R_IP_2; 39 | Eigen::Matrix K_pred; 40 | Eigen::Matrix H_; 41 | Eigen::Matrix P_pred; 42 | Eigen::Matrix STM_; 43 | Eigen::Matrix Q_; 44 | 45 | GpPredictor::Vector3 savePos, ins_enu_slip, ins_enu_slip3p, ins_enu_slip_3p; 46 | std_msgs::Float64 stop_cmd_msg_; 47 | 48 | bool new_gp_data_arrived_; 49 | bool gp_flag = false; 50 | double gp_arrived_time_; 51 | double xy_errSlip,odomUptCount, startRecording, stopRecording, saveCountOdom; 52 | double init_ecef_x,init_ecef_y,init_ecef_z,init_x, init_y, init_z; 53 | int slip_i=0; 54 | int i=0; 55 | 56 | private: 57 | ros::Subscriber gp_sub_; 58 | ros::Publisher stop_cmd_pub_; 59 | ros::ServiceClient clt_setStopping_; 60 | ros::NodeHandle & nh_; 61 | 62 | }; 63 | int main(int argc, char **argv); 64 | 65 | #endif //GP_PREDICTOR_H_ 66 | -------------------------------------------------------------------------------- /imu_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_filter 4 | 0.0.0 5 | The imu_filter package 6 | 7 | 8 | 9 | 10 | cagri 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | catkin 25 | geometry_msgs 26 | roscpp 27 | rospy 28 | sensor_msgs 29 | std_msgs 30 | message_filters 31 | geometry_msgs 32 | roscpp 33 | rospy 34 | sensor_msgs 35 | std_msgs 36 | geometry_msgs 37 | roscpp 38 | rospy 39 | sensor_msgs 40 | std_msgs 41 | message_filters 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /hw_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hw_interface 4 | 0.0.1 5 | The hw_interface package 6 | 7 | WVU Interactive Robotics Laboratory 8 | Matthew Gramlich/mpgramlich@mix.wvu.edu 9 | https://github.com/wvu-irl/fast-traverse-pathfinder 10 | https://web.statler.wvu.edu/~irl/ 11 | 12 | BSD 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 | catkin 39 | pluginlib 40 | roscpp 41 | pluginlib 42 | roscpp 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /core_navigation/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | init: 2 | orientation: 3 | # x: 0.0 4 | # y: 0.0 5 | # z: 0.0 6 | covx: 1.218469679146834e-06 7 | covy: 1.218469679146834e-06 8 | covz: 1.218469679146834e-06 9 | velocity: 10 | x: 0.0 11 | y: 0.0 12 | z: 0.0 13 | covx: 4.000000000000001e-07 14 | covy: 4.000000000000001e-07 15 | covz: 4.000000000000001e-07 16 | position: 17 | # x: 0.691949695095547 18 | # y: -1.395750757746275 19 | # z: 312.2643980319930 20 | covx: 0.0 21 | covy: 0.0 22 | covz: 0.0 23 | std_bias_a: 24 | x: 0.017333461987301 25 | y: 0.012647240119246 26 | z: 0.020974348023053 27 | std_bias_g: 28 | x: 0.000802891989877233 29 | y: 0.000904288959007464 30 | z: 0.000852026432825422 31 | # bias: 32 | # accel: 33 | # x: 7.549135895545244e-04 34 | # y: 0.001805886718248 35 | # z: 0.002395586011438 36 | # gyro: 37 | # x: 8.674066138832567e-05 38 | # y: 1.005391303275586e-04 39 | # z: 8.919962404601004e-05 40 | # ecef: 41 | # x: 8.565078494000000e+05 42 | # y: -4.842978088500000e+06 43 | # z: 4.047980191900000e+06 44 | 45 | imu: 46 | publish_hz: 50 47 | sensor_pub_rate: 50 48 | topic: /imu_filtered 49 | # topic: /imu/data 50 | noise: 51 | sig_gyro_inRun : 7.757018897752577e-06 52 | sig_ARW : 2.908882086657216e-05 53 | sig_accel_inRun : 3.139200000000000e-05 54 | sig_VRW : 1.333333333333333e-04 55 | 56 | odo: 57 | topic: /dead_reckoning/odometry 58 | updates: 59 | do_zupt: false 60 | do_zaru: false 61 | 62 | encoderLeft: 63 | topic: /roboteq/drivemotorin/left 64 | 65 | encoderRight: 66 | topic: /roboteq/drivemotorin/right 67 | cmd: 68 | topic: /cmd_vel_out 69 | joint: 70 | topic: /dead_reckoning/joint_states 71 | gp: 72 | topic: /core_nav/core_nav/gp_result 73 | stop: 74 | topic: /core_nav/core_nav/stop_cmd 75 | 76 | frames: 77 | frame_id_out: map 78 | frame_id_fixed: odom 79 | frame_id_imu: imu_link 80 | frame_id_odo: odom_link 81 | 82 | wheel: 83 | # T_r_: 0.125 84 | T_r_: 0.685 85 | s_or_: -0.07 86 | # s_or_: 0.0 87 | s_delta_or_: -0.6 88 | # s_delta_or_: 0.0 89 | -------------------------------------------------------------------------------- /core_navigation/script/utils/plot.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | 5 | def plot_co2(y_true, mean, lb, ub, trainlen, n, r): 6 | plt.plot(range(len(y_true)),y_true,'b',label="Actual") 7 | plt.plot(range(len(y_true)),mean,'r', label="ESN Prediction") 8 | plt.fill_between(range(len(y_true)), lb, ub, facecolor='grey', alpha=0.3) 9 | lo,hi = plt.ylim() 10 | plt.plot([trainlen,trainlen],[lo+np.spacing(1),hi-np.spacing(1)],'k:') 11 | plt.xlabel('Months since Aug 1960') 12 | plt.ylabel('CO2 Concentration (ppmv)') 13 | plt.legend(loc=2) 14 | plt.show() 15 | 16 | 17 | def plot_erie(y_true, mean, lb, ub, trainlen, n, r): 18 | plt.plot(range(len(y_true)),y_true,'b',label="Actual") 19 | plt.plot(range(len(y_true)),mean,'r', label="ESN Prediction") 20 | plt.fill_between(range(len(y_true)), lb, ub, facecolor='grey', alpha=0.3) 21 | lo,hi = plt.ylim() 22 | plt.plot([trainlen,trainlen],[lo+np.spacing(1),hi-np.spacing(1)],'k:') 23 | plt.xlabel('Months since Aug 1922') 24 | plt.ylabel('Water Level') 25 | plt.legend(loc=2) 26 | plt.show() 27 | 28 | def plot_airline(y_true, mean, lb, ub, trainlen, n, r): 29 | plt.plot(range(len(y_true)),y_true,'b',label="Target") 30 | plt.plot(range(len(y_true)),mean,'r', label="ESN n="+str(n)+", r="+str(r)) 31 | plt.fill_between(range(len(y_true)), lb, ub, facecolor='grey', alpha=0.3) 32 | lo,hi = plt.ylim() 33 | plt.plot([trainlen,trainlen],[lo+np.spacing(1),hi-np.spacing(1)],'k:') 34 | plt.xlabel('Time (months)') 35 | plt.ylabel('Number of Passengers') 36 | plt.legend(loc=2, fontsize='x-small') 37 | plt.show() 38 | 39 | 40 | def plot_solar(y_true, mean, lb, ub, trainlen, n, r): 41 | plt.plot(range(len(y_true)),y_true,'b',label="Target") 42 | plt.plot(range(len(y_true)),mean,'r', label="ESN n="+str(n)+", r="+str(r)) 43 | plt.fill_between(range(len(y_true)), lb, ub, facecolor='grey', alpha=0.3) 44 | lo,hi = plt.ylim() 45 | plt.plot([trainlen,trainlen],[lo+np.spacing(1),hi-np.spacing(1)],'k:') 46 | plt.xlabel('Years since 1629') 47 | plt.ylabel('TSI (w/m^2)') 48 | plt.legend(loc=2, fontsize='x-small') 49 | plt.show() 50 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR3_H 2 | #define GEOMETRY_UTILS_VECTOR3_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector3Base : VectorNBase { 10 | Vector3Base() : VectorNBase() {} 11 | Vector3Base(T val) : VectorNBase(val) {} 12 | Vector3Base(const Vector3Base& in) : VectorNBase(in.data) {} 13 | Vector3Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector3Base(T (&in)[3]) : VectorNBase(in) {} 15 | Vector3Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector3Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector3Base(T v1, T v2, T v3) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | this->data[2] = v3; 22 | } 23 | 24 | T X() const { return this->data[0]; } 25 | T Y() const { return this->data[1]; } 26 | T Z() const { return this->data[2]; } 27 | 28 | inline Vector3Base Cross(const Vector3Base& v) const { 29 | return Vector3Base(-(*this)(2) * v(1) + (*this)(1) * v(2), 30 | (*this)(2) * v(0) - (*this)(0) * v(2), 31 | -(*this)(1) * v(0) + (*this)(0) * v(1)); 32 | } 33 | }; 34 | 35 | inline Vector3Base operator*(const float& lhs, 36 | const Vector3Base& rhs) { 37 | return Vector3Base(rhs * lhs); 38 | } 39 | 40 | inline Vector3Base operator*(const double& lhs, 41 | const Vector3Base& rhs) { 42 | return Vector3Base(rhs * lhs); 43 | } 44 | 45 | template 46 | inline VectorNBase Cross(const VectorNBase& v1, 47 | const VectorNBase& v2) { 48 | return Vector3Base(v1).Cross(v2); 49 | } 50 | 51 | typedef Vector3Base Vector3f; 52 | typedef Vector3Base Vec3f; 53 | 54 | typedef Vector3Base Vector3d; 55 | typedef Vector3Base Vec3d; 56 | 57 | typedef Vector3Base Vector3; 58 | typedef Vector3Base Vec3; 59 | 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /pathfinder_control/launch/control.launch: -------------------------------------------------------------------------------- 1 | 2 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/MatrixNxNBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIXNXN_H 2 | #define GEOMETRY_UTILS_MATRIXNXN_H 3 | 4 | #include 5 | #include "VectorNBase.h" 6 | #include "GeometryUtilsMath.h" 7 | #include "MatrixNxMBase.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct MatrixNxNBase : MatrixNxMBase { 13 | MatrixNxNBase() : MatrixNxMBase() {} 14 | MatrixNxNBase(T val) : MatrixNxMBase(val) {} 15 | MatrixNxNBase(const MatrixNxNBase& in) : MatrixNxMBase(in.data) {} 16 | MatrixNxNBase(const boost::array& in) : MatrixNxMBase(in) {} 17 | MatrixNxNBase(T (&in)[N * N]) : MatrixNxMBase(in) {} 18 | MatrixNxNBase(const Eigen::Matrix& in) 19 | : MatrixNxMBase(in) {} 20 | MatrixNxNBase(const MatrixNxMBase& in) 21 | : MatrixNxMBase(in) {} 22 | 23 | inline void Eye() { 24 | this->data.fill(0); 25 | for (size_t i = 0; i < this->nrows; i++) 26 | this->data[this->nrows * i + i] = 1; 27 | } 28 | 29 | virtual inline T Det() const { 30 | std::cerr << "MatrixNxMBase::det not implemented" << std::endl; 31 | return T(); 32 | } 33 | 34 | virtual inline MatrixNxNBase Inv() const { 35 | std::cerr << "MatrixNxMBase::inv not implemented" << std::endl; 36 | return MatrixNxNBase(); 37 | } 38 | 39 | static inline MatrixNxNBase Diag(const VectorNBase& in) { 40 | T d[N * N] = {0}; 41 | for (size_t i = 0; i < N; i++) 42 | d[N * i + i] = in(i); 43 | return MatrixNxNBase(d); 44 | } 45 | }; 46 | 47 | template 48 | inline MatrixNxNBase operator*(const float& lhs, 49 | const MatrixNxNBase& rhs) { 50 | return MatrixNxNBase(rhs * lhs); 51 | } 52 | 53 | template 54 | inline MatrixNxNBase operator*(const double& lhs, 55 | const MatrixNxNBase& rhs) { 56 | return MatrixNxNBase(rhs * lhs); 57 | } 58 | 59 | template 60 | inline MatrixNxNBase Inv(const MatrixNxNBase& m) { 61 | return m.Inv(); 62 | } 63 | 64 | } 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /pathfinder_control/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pathfinder_control/src/drive_straight_with_stop.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | enum DRIVING_STATE_T {_waiting_for_stop, _stopped} driving_state = _waiting_for_stop; 6 | 7 | const double stop_duration = 5.0; // sec 8 | const double drive_speed = 1.0; // m/s 9 | const double stop_speed = 0.0; // m/s 10 | 11 | double time_to_stop; 12 | bool stop_commanded = false; 13 | 14 | void stopCallback(const std_msgs::Float64::ConstPtr& msg); 15 | 16 | int main(int argc, char** argv) 17 | { 18 | ros::init(argc, argv, "drive_straight_with_stop_node"); 19 | ros::NodeHandle nh; 20 | ros::Publisher pub = nh.advertise("cmd_vel", 1); 21 | ros::Subscriber sub = nh.subscribe("/core_nav/core_nav/stop_cmd", 1, stopCallback); 22 | ros::Rate loop_rate(20); 23 | geometry_msgs::Twist cmd_vel_msg; 24 | double stopped_time; 25 | 26 | while(ros::ok()) 27 | { 28 | switch(driving_state) 29 | { 30 | case _waiting_for_stop: 31 | if(stop_commanded==true) 32 | { 33 | ROS_INFO("remaining_time = %.3f",time_to_stop - ros::Time::now().toSec()); 34 | if(ros::Time::now().toSec() > time_to_stop) 35 | { 36 | stopped_time = ros::Time::now().toSec(); 37 | driving_state = _stopped; 38 | } 39 | } 40 | //cmd_vel_msg.linear.x = drive_speed; 41 | break; 42 | case _stopped: 43 | if(ros::Time::now().toSec() - stopped_time > stop_duration) 44 | { 45 | driving_state = _waiting_for_stop; 46 | stop_commanded = false; 47 | } 48 | cmd_vel_msg.linear.x = stop_speed; 49 | pub.publish(cmd_vel_msg); // Moved publisher here 50 | break; 51 | } 52 | // Publisher was here 53 | ros::spinOnce(); 54 | loop_rate.sleep(); 55 | } 56 | 57 | return 0; 58 | } 59 | 60 | void stopCallback(const std_msgs::Float64::ConstPtr& msg) 61 | { 62 | time_to_stop = ros::Time::now().toSec() + msg->data; 63 | stop_commanded = true; 64 | ROS_INFO("delta time = %.3f",msg->data); 65 | ROS_INFO("time to stop = %.3f",time_to_stop); 66 | } 67 | -------------------------------------------------------------------------------- /novatel_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | novatel_launch 4 | 0.0.0 5 | The novatel_launch package 6 | 7 | Cagri Kilic 8 | Cagri Kilic/cakilic@mix.wvu.edu 9 | 10 | BSD 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 | catkin 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /gps_data_log/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps_data_log 4 | 0.0.0 5 | The gps_data_log package 6 | 7 | 8 | 9 | 10 | nick 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 | catkin 43 | roscpp 44 | rospy 45 | message_generation 46 | message_runtime 47 | roscpp 48 | rospy 49 | message_generation 50 | message_runtime 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsSerialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_SERIALIZATION_H 22 | #define GEOMETRY_UTILS_SERIALIZATION_H 23 | 24 | #include "GeometryUtils.h" 25 | 26 | #include 27 | #include 28 | 29 | namespace boost { 30 | namespace serialization { 31 | 32 | template 33 | void Serialize(Archive & ar, geometry_utils::Vector2& v, 34 | const unsigned int version) { 35 | ar & v(0); 36 | ar & v(1); 37 | } 38 | 39 | template 40 | void Serialize(Archive & ar, geometry_utils::Vector3& v, 41 | const unsigned int version) { 42 | ar & v(0); 43 | ar & v(1); 44 | ar & v(2); 45 | } 46 | 47 | template 48 | void Serialize(Archive & ar, geometry_utils::Vector4& v, 49 | const unsigned int version) { 50 | ar & v(0); 51 | ar & v(1); 52 | ar & v(2); 53 | ar & v(3); 54 | } 55 | 56 | template 57 | void Serialize(Archive & ar, geometry_utils::Matrix3x3& m, 58 | const unsigned int version) { 59 | for (unsigned int i = 0; i < 3; i++) 60 | for (unsigned int j = 0; j < 3; j++) 61 | ar & m(i, j); 62 | } 63 | 64 | template 65 | void Serialize(Archive & ar, geometry_utils::Transform& t, 66 | const unsigned int version) { 67 | ar & t.translation; 68 | ar & t.rotation; 69 | } 70 | 71 | } 72 | } 73 | #endif 74 | -------------------------------------------------------------------------------- /VIO Analysis/README.md: -------------------------------------------------------------------------------- 1 | ## Overview 2 | 3 | **Author: Cagri Kilic
4 | Affiliation: [WVU NAVLAB](https://navigationlab.wvu.edu/)
5 | Maintainer: Cagri Kilic, cakilic@mix.wvu.edu** 6 | 7 | Supplementary Visual-Inertial Odometry (VIO) Analyses in low-feature environment for the Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Proprioceptive Localization paper. 8 | 9 | The VIO solution is generated using ROS Wrapper for Intel® RealSense™ Devices https://github.com/IntelRealSense/realsense-ros 10 | 11 | Each figure provides a DGPS solution, filter estimation (corenav-GP solution), 2D Dead Reckoning (only WO and IMU heading). The Google Map representation of the ground truth (DGPS) solution is provided to visualize the low-feature environment (Point Marion, PA, Ashpiles Mars Analog Environment). Solution accuracy values are given for the corenav-GP implementation. 12 | 13 | ### Scenario 1, Execution 1 14 |

15 | architecture 16 |

17 | 18 | *VIO failed after traversing 124m 19 | 20 | ### Scenario 1, Execution 2 21 |

22 | architecture 23 |

24 | 25 | 26 | ### Scenario 1, Execution 3 27 |

28 | architecture 29 |

30 | 31 | ### Scenario 1 Analysis 32 |

33 | architecture 34 |

35 | 36 | 37 | The median values (out of 3 execution) of RMSE for CoreNav-GP are E=1.03m, N=0.49m, U=0.65m 38 | 39 | The median values (out of 3 execution) of RMSE for VIO are E=2.70m, N=12.22*, U=2.12m 40 | 41 | In Execution 1, there is only 124m out of 150m traversed solution for VIO. After 124m, VIO failed and outputs NaN values. For this reason, the star (*) values show the accuracy until 124 m. 42 | 43 | 44 | ### Additional Scenario 1 45 |

46 | architecture 47 |

48 | 49 | The values of RMSE for CoreNav-GP are E=0.66m, N=0.40m, U=2.89m 50 | 51 | The values of RMSE for VIO are E=13.27m, N=56.62m, U=2.12m 52 | 53 | ### Additional Scenario 2 54 |

55 | architecture 56 |

57 | 58 | The values of RMSE for CoreNav-GP are E=1.23m, N=2.51m, U=1.97m 59 | 60 | The values of RMSE for VIO are E=44.04m, N=27.24m, U=0.42m 61 | 62 | 63 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/include/hw_interface_plugin_adis_imu/hw_interface_plugin_adis_imu_serial.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HW_INTERFACE_PLUGIN_ADIS_IMU_HPP__ 2 | #define HW_INTERFACE_PLUGIN_ADIS_IMU_HPP__ 3 | 4 | //always inlclude these 5 | #include 6 | #include 7 | #include 8 | 9 | //include the header of the base type you want, Serial or UDP 10 | #include 11 | //include ros message types 12 | #include 13 | //TODO 14 | //#include 15 | //#include 16 | 17 | #define CRC32_POLYNOMIAL 0xEDB88320L 18 | #define PI 3.14159265358979 19 | #define RAD2DEG 180.0/PI 20 | #define DEG2RAD PI/180.0 21 | #define IMU_RATE 125.0 // Hz 22 | 23 | namespace hw_interface_plugin_adis_imu { 24 | 25 | class adis_imu_serial : public base_classes::base_serial_interface 26 | { 27 | public: 28 | adis_imu_serial(); 29 | ~adis_imu_serial() {} 30 | 31 | protected: 32 | 33 | //these methods are abstract as defined by the base_serial_interface 34 | //they must be defined 35 | bool subPluginInit(ros::NodeHandlePtr nhPtr); 36 | void setInterfaceOptions(); 37 | bool interfaceReadHandler(const size_t &length, int arrayStartPos, const boost::system::error_code &ec); 38 | bool verifyChecksum(); 39 | 40 | std::size_t adisIMUStreamMatcher(const boost::system::error_code &error, long totalBytesInBuffer); 41 | 42 | long headerLen; 43 | long fullPacketLen; 44 | sensor_msgs::Imu imuMessage; 45 | sensor_msgs::Imu imuMessageAdis; 46 | //TODO 47 | //sensor_msgs::MagneticField magMessage 48 | //sensor_msgs::FluidPressure barMessage 49 | ros::Publisher imuPublisher; 50 | ros::Publisher imuPublisherAdis; 51 | //TODO 52 | //ros::Publisher magPublisher; 53 | //ros::Publisher barPublisher; 54 | double gyroScaleFactor; 55 | double accelScaleFactor; 56 | 57 | private: 58 | unsigned long CRC32Value_(int i); 59 | unsigned long CalculateBlockCRC32_(unsigned long ulCount, unsigned char *ucBuffer ); // Number of bytes in the data block, Data block 60 | }; 61 | 62 | } 63 | 64 | PLUGINLIB_EXPORT_CLASS(hw_interface_plugin_adis_imu::adis_imu_serial, base_classes::base_interface) 65 | 66 | 67 | 68 | #endif //HW_INTERFACE_PLUGIN_ADIS_IMU_HPP__ 69 | 70 | -------------------------------------------------------------------------------- /pathfinder_control/launch/control_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/include/hw_interface_plugin_roboteq/hw_interface_plugin_roboteq_brushed.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef HW_INTERFACE_PLUGIN_ROBOTEQ_BRUSHED_HPP__ 37 | #define HW_INTERFACE_PLUGIN_ROBOTEQ_BRUSHED_HPP__ 38 | 39 | #include 40 | 41 | namespace hw_interface_plugin_roboteq 42 | { 43 | class brushed : public hw_interface_plugin_roboteq::roboteq_serial 44 | { 45 | public: 46 | brushed(); 47 | 48 | protected: 49 | bool implStart(); 50 | bool implStop(); 51 | bool implDataHandler(); 52 | 53 | }; 54 | } 55 | 56 | PLUGINLIB_EXPORT_CLASS(hw_interface_plugin_roboteq::brushed, base_classes::base_interface) 57 | 58 | #endif //HW_INTERFACE_PLUGIN_ROBOTEQ_BRUSHED_HPP__ 59 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hw_interface_plugin_roboteq 4 | 0.0.0 5 | The hw_interface_plugin_roboteq package 6 | 7 | WVU Interactive Robotics Laboratory 8 | Jennifer Nguyen/jqnguyen@mix.wvu.edu 9 | Matthew Gramlich/mpgramlich@mix.wvu.edu 10 | Nicholas Ohi/nohi@mix.wvu.edu 11 | https://github.com/wvu-irl/fast-traverse-pathfinder 12 | https://web.statler.wvu.edu/~irl/ 13 | 14 | 15 | 16 | 17 | 18 | TODO 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 | catkin 45 | hw_interface 46 | pluginlib 47 | roscpp 48 | message_generation 49 | hw_interface 50 | pluginlib 51 | roscpp 52 | message_generation 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Rotation2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_ROTATION2_H 2 | #define GEOMETRY_UTILS_ROTATION2_H 3 | 4 | #include 5 | #include "GeometryUtilsMath.h" 6 | #include "RotationNBase.h" 7 | #include "Matrix2x2.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct Rotation2Base : RotationNBase { 13 | Rotation2Base() : RotationNBase() {} 14 | Rotation2Base(const Rotation2Base& in) : RotationNBase(in.data) {} 15 | Rotation2Base(const boost::array& in) : RotationNBase(in) {} 16 | Rotation2Base(T (&in)[2 * 2]) : RotationNBase(in) {} 17 | Rotation2Base(const Eigen::Matrix& in) : RotationNBase(in) {} 18 | Rotation2Base(const Eigen::Rotation2D& in) 19 | : RotationNBase(in.toRotationMatrix()) {} 20 | Rotation2Base(const RotationNBase& in) : RotationNBase(in) {} 21 | Rotation2Base(const Matrix2x2Base& in) : RotationNBase(in) {} 22 | Rotation2Base(const MatrixNxMBase& in) : RotationNBase(in) {} 23 | 24 | Rotation2Base(T val) { FromAngle(val); } 25 | 26 | Rotation2Base(T R11, T R12, T R21, T R22) { 27 | this->data[0] = R11; 28 | this->data[1] = R12; 29 | this->data[2] = R21; 30 | this->data[3] = R22; 31 | } 32 | 33 | virtual inline bool Equals(const Rotation2Base& that, 34 | const T ptol = 1e-8) const { 35 | return Error(that) < ptol; 36 | } 37 | 38 | inline T Error(const Rotation2Base& r) const { 39 | return math::sin(Angle() - r.Angle()); 40 | } 41 | 42 | inline T Angle() const { 43 | return math::atan2(this->data[2], this->data[0]); 44 | } 45 | 46 | inline void FromAngle(T val) { 47 | this->data[0] = math::cos(val); 48 | this->data[1] = -math::sin(val); 49 | this->data[2] = math::sin(val); 50 | this->data[3] = math::cos(val); 51 | } 52 | 53 | inline Eigen::Rotation2D Eigen() { 54 | return Eigen::Rotation2D(Angle()); 55 | } 56 | }; 57 | 58 | inline Rotation2Base operator*(const float& lhs, 59 | const Rotation2Base& rhs) { 60 | return Rotation2Base(rhs.Scale(lhs)); 61 | } 62 | 63 | inline Rotation2Base operator*(const double& lhs, 64 | const Rotation2Base& rhs) { 65 | return Rotation2Base(rhs.Scale(lhs)); 66 | } 67 | 68 | template 69 | inline Eigen::Rotation2D Eigen(const Rotation2Base& in) { 70 | return in.Eigen(); 71 | } 72 | 73 | typedef Rotation2Base Rotation2f; 74 | typedef Rotation2Base Rot2f; 75 | 76 | typedef Rotation2Base Rotation2d; 77 | typedef Rotation2Base Rot2d; 78 | 79 | typedef Rotation2Base Rotation2; 80 | typedef Rotation2Base Rot2; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_adis_imu/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hw_interface_plugin_adis_imu 4 | 0.0.1 5 | ADIS IMU HW_Interface Package 6 | 7 | 8 | 9 | 10 | ros 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Cagri Kilic 29 | Nick Ohi 30 | Matt Gramlich 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | 45 | hw_interface 46 | pluginlib 47 | roscpp 48 | sensor_msgs 49 | 50 | hw_interface 51 | pluginlib 52 | roscpp 53 | sensor_msgs 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /core_navigation/script/gp_slip_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import GPy 4 | import rospy 5 | import numpy as np 6 | #from matplotlib import pyplot as plt 7 | #from utils.data import * 8 | from core_nav.msg import GP_Input 9 | from core_nav.msg import GP_Output 10 | #from IPython.display import display 11 | 12 | pub = rospy.Publisher('/core_nav/core_nav/gp_result', GP_Output, queue_size=1) 13 | 14 | 15 | 16 | def callback(data): 17 | # Do GP stuff here ********* 18 | rospy.loginfo("GP Input Arrived") 19 | X = data.time_array 20 | #X = [1,2,3,4,5] 21 | X = np.array(X).reshape(len(X), 1) 22 | 23 | Y = data.slip_array 24 | #Y = [0.1,0.05,-0.009,0.1,-0.002] 25 | Y = np.array(Y).reshape(len(Y), 1) 26 | 27 | per = 0.9 28 | 29 | x_train, y_train = X[:int(per * len(X))], Y[:int(per * len(Y))] 30 | x_test, y_test = X[int(per * len(X)):], Y[int(per * len(Y)):] 31 | kernel = (GPy.kern.RBF(1)) * GPy.kern.Brownian(1) 32 | # # GPy.kern.Matern32(1), GPy.kern.Matern52(1), GPy.kern.Brownian(1), 33 | # # GPy.kern.Bias(1), GPy.kern.Linear(1), GPy.kern.PeriodicExponential(1), 34 | # # GPy.kern.White(1)] 35 | m = GPy.models.GPRegression(x_train, y_train, kernel) 36 | m.optimize(messages=False) 37 | 38 | 39 | # display(m) 40 | means = [] # predictions 41 | variances = [] # uncertainty 42 | #means2= [] #predictions 43 | #variances2 = [] #uncertainty 44 | # X_=np.arange(X.min(),X.max()+50, 0.1) 45 | X_ = np.arange(X.min(), X.max() + 600, 1) 46 | 47 | for x in X_: 48 | mean, covar = m.predict(np.array([[x]])) 49 | variances.append(covar[0]) 50 | means.append(mean[0]) 51 | #means2.append(mean[0]) 52 | #variances2.append(covar[0]) 53 | 54 | # ************************** 55 | 56 | # Write output arrays into msg_out.mean and msg_out.sigma 57 | msg_out = GP_Output() 58 | # TODO: need to populate the header (i.e. msg_out.header) 59 | msg_out.mean = np.array(means[int(len(X)):]) # [0,1,3,4] # for example 60 | # [6,7,8,9] # for example 61 | msg_out.sigma = 2 * np.sqrt(np.array(variances[int(len(X)):])) 62 | rospy.loginfo("Publishing GP Results") 63 | pub.publish(msg_out) 64 | 65 | #ses = 2 * np.sqrt(np.array(variances2)) 66 | #means = np.array(means2) 67 | #var1 = np.array(means + ses).reshape(1, len(means + ses)) 68 | #var2 = np.array(means - ses).reshape(1, len(means - ses)) 69 | # m.plot() 70 | # plt.xlim(X.min(), X_.max()) 71 | # plt.plot(X, Y, c='r', linewidth=0.2) 72 | # # confidence intervals #C0C0C0 73 | # plt.fill_between(X_, var2[0, :], var1[0, :], alpha=0.3, color='k') 74 | # plt.plot(x_test, y_test, 'xk') 75 | # plt.ylim(-1.5, 1.5) 76 | # plt.show() 77 | 78 | 79 | def gaussian_process(): 80 | rospy.init_node('gp_slip_node') 81 | rospy.Subscriber('/core_nav/core_nav/gp_input', GP_Input, callback) 82 | #rospy.loginfo_once("GP Mode Has Started") 83 | rospy.spin() 84 | 85 | 86 | if __name__ == '__main__': 87 | gaussian_process() 88 | -------------------------------------------------------------------------------- /geometry_utils/tests/test_math.cc: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_DYN_LINK 2 | #define BOOST_TEST_MODULE test_math 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace gu = geometry_utils; 9 | namespace gm = gu::math; 10 | 11 | BOOST_AUTO_TEST_CASE(math) { 12 | srand(time(NULL)); 13 | float rf = static_cast(rand())/static_cast(RAND_MAX); 14 | double rd = static_cast(rand())/static_cast(RAND_MAX); 15 | 16 | BOOST_CHECK_EQUAL(gm::cos(rf), cosf(rf)); 17 | BOOST_CHECK_EQUAL(gm::cos(rf), gm::cos(rf)); 18 | BOOST_CHECK_EQUAL(gm::cos(rd), cos(rd)); 19 | BOOST_CHECK_EQUAL(gm::cos(rd), gm::cos(rd)); 20 | 21 | BOOST_CHECK_EQUAL(gm::acos(rf), acosf(rf)); 22 | BOOST_CHECK_EQUAL(gm::acos(rf), gm::acos(rf)); 23 | BOOST_CHECK_EQUAL(gm::acos(rd), acos(rd)); 24 | BOOST_CHECK_EQUAL(gm::acos(rd), gm::acos(rd)); 25 | 26 | BOOST_CHECK_EQUAL(gm::sin(rf), sinf(rf)); 27 | BOOST_CHECK_EQUAL(gm::sin(rf), gm::sin(rf)); 28 | BOOST_CHECK_EQUAL(gm::sin(rd), sin(rd)); 29 | BOOST_CHECK_EQUAL(gm::sin(rd), gm::sin(rd)); 30 | 31 | BOOST_CHECK_EQUAL(gm::asin(rf), asinf(rf)); 32 | BOOST_CHECK_EQUAL(gm::asin(rf), gm::asin(rf)); 33 | BOOST_CHECK_EQUAL(gm::asin(rd), asin(rd)); 34 | BOOST_CHECK_EQUAL(gm::asin(rd), gm::asin(rd)); 35 | 36 | BOOST_CHECK_EQUAL(gm::tan(rf), tanf(rf)); 37 | BOOST_CHECK_EQUAL(gm::tan(rf), gm::tan(rf)); 38 | BOOST_CHECK_EQUAL(gm::tan(rd), tan(rd)); 39 | BOOST_CHECK_EQUAL(gm::tan(rd), gm::tan(rd)); 40 | 41 | BOOST_CHECK_EQUAL(gm::fabs(rf), fabsf(rf)); 42 | BOOST_CHECK_EQUAL(gm::fabs(rf), gm::fabs(rf)); 43 | BOOST_CHECK_EQUAL(gm::fabs(rd), fabs(rd)); 44 | BOOST_CHECK_EQUAL(gm::fabs(rd), gm::fabs(rd)); 45 | 46 | BOOST_CHECK_EQUAL(gm::sqrt(rf), sqrtf(rf)); 47 | BOOST_CHECK_EQUAL(gm::sqrt(rf), gm::sqrt(rf)); 48 | BOOST_CHECK_EQUAL(gm::sqrt(rd), sqrt(rd)); 49 | BOOST_CHECK_EQUAL(gm::sqrt(rd), gm::sqrt(rd)); 50 | 51 | float rf1 = static_cast(rand())/static_cast(RAND_MAX); 52 | float rf2 = static_cast(rand())/static_cast(RAND_MAX); 53 | double rd1 = static_cast(rand())/static_cast(RAND_MAX); 54 | double rd2 = static_cast(rand())/static_cast(RAND_MAX); 55 | 56 | BOOST_CHECK_EQUAL(gm::atan2(rf1, rf2), atan2f(rf1, rf2)); 57 | BOOST_CHECK_EQUAL(gm::atan2(rf1, rf2), gm::atan2(rf1, rf2)); 58 | BOOST_CHECK_EQUAL(gm::atan2(rd1, rd2), atan2(rd1, rd2)); 59 | BOOST_CHECK_EQUAL(gm::atan2(rd1, rd2), gm::atan2(rd1, rd2)); 60 | 61 | BOOST_CHECK_EQUAL(gm::hypot(rf1, rf2), hypotf(rf1, rf2)); 62 | BOOST_CHECK_EQUAL(gm::hypot(rf1, rf2), gm::hypot(rf1, rf2)); 63 | BOOST_CHECK_EQUAL(gm::hypot(rd1, rd2), hypot(rd1, rd2)); 64 | BOOST_CHECK_EQUAL(gm::hypot(rd1, rd2), gm::hypot(rd1, rd2)); 65 | } 66 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | logo 3 |

4 | 5 | 6 | ## Overview 7 | 8 | **Author: Cagri Kilic
9 | Affiliation: [WVU NAVLAB](https://navigationlab.wvu.edu/)
10 | Maintainer: Cagri Kilic, cakilic@mix.wvu.edu** 11 | 12 | Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Localization 13 | 14 | ## Architecture 15 |

16 | architecture 17 |

18 | 19 | **Keywords:** Mars Sample Return, planetary rovers, time series prediction, slip, zero velocity update, rover localization 20 | 21 | 22 | ## Citation 23 | 24 | If you find this library useful, please cite the following publication: 25 | 26 | **[1]** Cagri Kilic, Nicholas Ohi, Yu Gu, Jason N. Gross: **Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Localization**. IEEE Robotics and Automation Letters (RA-L), 2021. https://doi.org/10.1109/LRA.2021.3068893 27 | 28 | @article{Kilic2021, 29 | author={Kilic, Cagri and Ohi, Nicholas and and Gu, Yu and Gross, Jason N}, 30 | journal={IEEE Robotics and Automation Letters}, 31 | title={Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Localization}, 32 | year={2021}, 33 | volume={6}, 34 | number={3}, 35 | pages={4782-4789}, 36 | doi={10.1109/LRA.2021.3068893} 37 | } 38 | 39 | 40 | **[2]** Cagri Kilic and Jason Gross: **Pathfinder GPS, IMU, and Wheel Odometry Data on Various Terrains**, IEEE Dataport, November 23, 2020, doi: https://dx.doi.org/10.21227/vz7z-jc84. 41 | 42 | @data{Kilic2020Pathfinder, 43 | doi = {10.21227/vz7z-jc84}, 44 | url = {https://dx.doi.org/10.21227/vz7z-jc84}, 45 | author = {Cagri Kilic; Jason Gross }, 46 | publisher = {IEEE Dataport}, 47 | title = {Pathfinder GPS, IMU, and Wheel Odometry Data on Various Terrains}, 48 | year = {2020} } 49 | 50 | } 51 | 52 | **[3]** Cagri Kilic, Jason N. Gross, Nicholas Ohi, Ryan Watson, Jared Strader, Thomas Swiger, Scott Harper, and Yu Gu: **Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints**. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019. https://doi.org/10.1109/IROS40897.2019.8967634 53 | 54 | @inproceedings{Kilic2019, 55 | author = {Kilic, Cagri and Gross, Jason N. and Ohi, Nicholas and Watson, Ryan and Strader, Jared and Swiger, Thomas and Harper, Scott and Gu, Yu}, 56 | booktitle = {2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 57 | title = {{Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints}}, 58 | year = {2019}, 59 | pages={552-559}, 60 | doi={10.1109/IROS40897.2019.8967634}, 61 | ISSN={2153-0858}, 62 | publisher = {IEEE}, 63 | 64 | } 65 | -------------------------------------------------------------------------------- /core_navigation/src/core_nav.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "inertial propagation"); 6 | ros::NodeHandle n("~"); 7 | 8 | CoreNav coreNav; 9 | if(!coreNav.Initialize(n)) { 10 | ROS_ERROR("%s: Failed to initialize the nav. filter.", 11 | ros::this_node::getName().c_str()); 12 | return EXIT_FAILURE; 13 | } 14 | 15 | 16 | // while(ros::ok()) 17 | // { 18 | // if(coreNav.propagate_flag) 19 | // { 20 | // // ROS_INFO("Propagate flag: Before Propagate\n"); 21 | // coreNav.Propagate(); 22 | // // ROS_INFO("Propagate flag: After Propagate\n"); 23 | // coreNav.propagate_flag =false; 24 | // } 25 | // 26 | // if(coreNav.update_flag) 27 | // { 28 | // // ROS_INFO("Update Flag: before update\n"); 29 | // coreNav.Update(); 30 | // // ROS_INFO("Update Flag: after update\n"); 31 | // coreNav.update_flag =false; 32 | // } 33 | // ros::spinOnce(); 34 | // } 35 | 36 | ros::spin(); 37 | 38 | return EXIT_SUCCESS; 39 | } 40 | 41 | 42 | // #include 43 | // #include 44 | // 45 | // int main(int argc, char** argv){ 46 | // ros::init(argc, argv, "inertial propagation"); 47 | // ros::NodeHandle n("~"); 48 | // 49 | // CoreNav coreNav; 50 | // if(!coreNav.Initialize(n)) { 51 | // ROS_ERROR("%s: Failed to initialize the nav. filter.", 52 | // ros::this_node::getName().c_str()); 53 | // return EXIT_FAILURE; 54 | // } 55 | // // ros::MultiThreadedSpinner spinner(0); 56 | // // spinner.spin(); 57 | // // ros::AsyncSpinner spinner(0); 58 | // // spinner.start(); 59 | // while(ros::ok()) 60 | // { 61 | // if(coreNav.propagate_flag) 62 | // { 63 | // // ROS_INFO("Propagate flag: Before Propagate\n"); 64 | // // coreNav.Propagate(coreNav.imu,coreNav.odo,coreNav.cmd,coreNav.encoderLeft,coreNav.encoderRight,coreNav.joint); 65 | // coreNav.Propagate(); 66 | // 67 | // // ROS_INFO("Propagate flag: After Propagate\n"); 68 | // coreNav.propagate_flag =false; 69 | // } 70 | // 71 | // if(coreNav.update_flag) 72 | // { 73 | // // ROS_INFO("Update Flag: before update\n"); 74 | // // coreNav.Update(coreNav.odo,coreNav.joint); 75 | // coreNav.Update(); 76 | // 77 | // // ROS_INFO("Update Flag: after update\n"); 78 | // coreNav.update_flag =false; 79 | // } 80 | // ros::spinOnce(); 81 | // } 82 | // // ros::spin(); 83 | // 84 | // // ros::waitForShutdown(); 85 | // return EXIT_SUCCESS; 86 | // } 87 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/YAMLLoader.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_YAML_LOADER_H 22 | #define GEOMETRY_UTILS_YAML_LOADER_H 23 | 24 | #ifdef __YAML_UTILS_H__ 25 | #error "YAMLUtils must be included last" 26 | #endif 27 | 28 | #include 29 | #include 30 | 31 | namespace YAML { 32 | 33 | template <> 34 | struct convert { 35 | static Node Encode(const geometry_utils::Vector2& rhs) { 36 | Node node; 37 | node.push_back(rhs[0]); 38 | node.push_back(rhs[1]); 39 | return node; 40 | } 41 | 42 | static bool Decode(const Node& node, geometry_utils::Vector2& rhs) { 43 | if (!node.IsSequence() || node.size() != 2) 44 | return false; 45 | 46 | rhs[0] = node[0].as(); 47 | rhs[1] = node[1].as(); 48 | return true; 49 | } 50 | }; 51 | 52 | template <> 53 | struct convert { 54 | static Node Encode(const geometry_utils::Vector3& rhs) { 55 | Node node; 56 | node.push_back(rhs[0]); 57 | node.push_back(rhs[1]); 58 | node.push_back(rhs[2]); 59 | return node; 60 | } 61 | 62 | static bool Decode(const Node& node, geometry_utils::Vector3& rhs) { 63 | if (!node.IsSequence() || node.size() != 3) 64 | return false; 65 | 66 | rhs[0] = node[0].as(); 67 | rhs[1] = node[1].as(); 68 | rhs[2] = node[2].as(); 69 | return true; 70 | } 71 | }; 72 | 73 | template <> 74 | struct convert { 75 | static Node Encode(const geometry_utils::Vector4& rhs) { 76 | Node node; 77 | node.push_back(rhs[0]); 78 | node.push_back(rhs[1]); 79 | node.push_back(rhs[2]); 80 | node.push_back(rhs[3]); 81 | return node; 82 | } 83 | 84 | static bool Decode(const Node& node, geometry_utils::Vector4& rhs) { 85 | if (!node.IsSequence() || node.size() != 4) 86 | return false; 87 | 88 | rhs[0] = node[0].as(); 89 | rhs[1] = node[1].as(); 90 | rhs[2] = node[2].as(); 91 | rhs[3] = node[3].as(); 92 | return true; 93 | } 94 | }; 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /gp_predictor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gp_predictor 4 | 0.0.0 5 | The gp_predictor package 6 | 7 | 8 | 9 | 10 | cagri 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 | roscpp 53 | std_msgs 54 | core_nav 55 | 56 | roscpp 57 | std_msgs 58 | core_nav 59 | 60 | roscpp 61 | std_msgs 62 | core_nav 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Quaternion.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_QUATERNION_H 2 | #define GEOMETRY_UTILS_QUATERNION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "GeometryUtilsMath.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct QuaternionBase : VectorNBase { 13 | QuaternionBase() : VectorNBase() { 14 | this->data.assign(0); 15 | this->data[0] = 1; 16 | } 17 | 18 | QuaternionBase(T val) : VectorNBase(val) {} 19 | QuaternionBase(const QuaternionBase& in) : VectorNBase(in.data) {} 20 | QuaternionBase(const boost::array& in) : VectorNBase(in) {} 21 | QuaternionBase(T (&in)[4]) : VectorNBase(in) {} 22 | QuaternionBase(const Eigen::Quaternion& in) { 23 | this->data[0] = in.w(); 24 | this->data[1] = in.x(); 25 | this->data[2] = in.y(); 26 | this->data[3] = in.z(); 27 | } 28 | QuaternionBase(const VectorNBase& in) : VectorNBase(in) {} 29 | 30 | QuaternionBase(T w, T x, T y, T z) { 31 | this->data[0] = w; 32 | this->data[1] = x; 33 | this->data[2] = y; 34 | this->data[3] = z; 35 | } 36 | 37 | inline QuaternionBase operator*(const QuaternionBase& rhs) const { 38 | T a1 = this->data[0]; 39 | T b1 = this->data[1]; 40 | T c1 = this->data[2]; 41 | T d1 = this->data[3]; 42 | T a2 = rhs.data[0]; 43 | T b2 = rhs.data[1]; 44 | T c2 = rhs.data[2]; 45 | T d2 = rhs.data[3]; 46 | return QuaternionBase(a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2, 47 | a2 * b1 + a1 * b2 - c2 * d1 + c1 * d2, 48 | a2 * c1 + a1 * c2 + b2 * d1 - b1 * d2, 49 | -(b2 * c1) + b1 * c2 + a2 * d1 + a1 * d2); 50 | } 51 | 52 | inline T& W() { return this->data[0]; } 53 | inline const T& W() const { return this->data[0]; } 54 | 55 | inline T& X() { return this->data[1]; } 56 | inline const T& X() const { return this->data[1]; } 57 | 58 | inline T& Y() { return this->data[2]; } 59 | inline const T& Y() const { return this->data[2]; } 60 | 61 | inline T& Z() { return this->data[3]; } 62 | inline const T& Z() const { return this->data[3]; } 63 | 64 | inline QuaternionBase Conj() const { 65 | return QuaternionBase(this->data[0], -this->data[1], -this->data[2], 66 | -this->data[3]); 67 | } 68 | 69 | inline QuaternionBase Error(const QuaternionBase& q) const { 70 | return q * (*this).Conj(); 71 | } 72 | 73 | inline QuaternionBase AxisAngle() const { 74 | QuaternionBase q((*this)); 75 | if (q.W() > 1) 76 | q = q.Normalize(); 77 | T den = math::sqrt(1 - q.W() * q.W()); 78 | if (den < 1e-6) 79 | den = 1; 80 | return QuaternionBase(2 * math::acos(q.W()), q.X() / den, q.Y() / den, 81 | q.Z() / den); 82 | } 83 | }; 84 | 85 | typedef QuaternionBase Quaternionf; 86 | typedef QuaternionBase Quatf; 87 | 88 | typedef QuaternionBase Quaterniond; 89 | typedef QuaternionBase Quatd; 90 | 91 | typedef QuaternionBase Quaternion; 92 | typedef QuaternionBase Quat; 93 | 94 | } 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /geometry_utils/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /hw_interface/include/hw_interface/bit_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef BIT_UTILS_H 37 | #define BIT_UTILS_H 38 | 39 | class Utils_Base 40 | { 41 | public: 42 | int zero_one(int input_val) {if (input_val == 0) return 0; else return 1;} 43 | }; 44 | 45 | class Set_Reset:public Utils_Base 46 | { 47 | private: 48 | int val; 49 | public: 50 | Set_Reset() {val = 0;} 51 | int set(int input_val) {if (zero_one(input_val)==1) val = 1; return val;} 52 | int reset(int input_val) {if (zero_one(input_val)==1) val = 0; return val;} 53 | int get_val() {return val;} 54 | }; 55 | 56 | class Leading_Edge_Latch:public Utils_Base 57 | { 58 | private: 59 | int last_val; 60 | int LE_val; 61 | int first_pass; 62 | public: 63 | Leading_Edge_Latch() {last_val = 0; LE_val = 0; first_pass = 1;} 64 | int LE_Latch(int input_val) {if(first_pass==1){last_val = zero_one(input_val);} LE_val = (zero_one(input_val)==1/*!=last_val*/) && (last_val==0); last_val = zero_one(input_val); first_pass = 0; return LE_val;} 65 | int get_val() {return LE_val;} 66 | }; 67 | 68 | class Trailing_Edge_Latch:public Utils_Base 69 | { 70 | private: 71 | int last_val; 72 | int TE_val; 73 | public: 74 | Trailing_Edge_Latch() {last_val = 0; TE_val = 0;} 75 | int TE_Latch(int input_val) {TE_val = (zero_one(input_val)!=last_val) && (last_val==1); last_val = input_val; return TE_val;} 76 | int get_val() {return TE_val;} 77 | }; 78 | #endif /* BIT_UTILS_H */ 79 | -------------------------------------------------------------------------------- /parameter_utils/include/parameter_utils/ParameterUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | parameter_utils: Convenience utility functions for working with ROS parameters 3 | Copyright (C) 2013 Nathan Michael 4 | 2015 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef PARAMETER_UTILS_H 22 | #define PARAMETER_UTILS_H 23 | 24 | #include 25 | #include 26 | 27 | namespace ros { 28 | namespace param { 29 | inline bool get(const std::string& key, unsigned char& c) { 30 | int i; 31 | bool ret = ros::param::get(key, i); 32 | assert((i >= 0) && (i < 256)); 33 | c = (unsigned char)i; 34 | return ret; 35 | } 36 | 37 | inline bool get(const std::string& key, unsigned int& d) { 38 | int i; 39 | bool ret = ros::param::get(key, i); 40 | assert(i >= 0); 41 | d = (unsigned int)i; 42 | return ret; 43 | } 44 | 45 | inline bool get(const std::string& key, float& f) { 46 | double d; 47 | bool ret = ros::param::get(key, d); 48 | f = (float)d; 49 | return ret; 50 | } 51 | } 52 | } 53 | 54 | namespace parameter_utils { 55 | template 56 | bool Get(const std::string& s, M& p) { 57 | std::string name = ros::this_node::getName(); 58 | 59 | std::string r; 60 | if (!ros::param::search(s, r)) { 61 | ROS_ERROR("%s: Failed to search for parameter '%s'.", name.c_str(), 62 | s.c_str()); 63 | return false; 64 | } 65 | 66 | if (!ros::param::has(r)) { 67 | ROS_ERROR("%s: Missing required parameter '%s'.", name.c_str(), s.c_str()); 68 | return false; 69 | } 70 | 71 | if (!ros::param::get(r, p)) { 72 | ROS_ERROR("%s: Failed to get parameter '%s'.", name.c_str(), s.c_str()); 73 | return false; 74 | } 75 | 76 | return true; 77 | } 78 | 79 | template 80 | bool Get(const std::string& s, M& p, M def) { 81 | bool ret = true; 82 | 83 | std::string name = ros::this_node::getName(); 84 | 85 | std::string r; 86 | if (!ros::param::search(s, r)) { 87 | ROS_DEBUG("%s: Failed to search for parameter '%s', using default.", 88 | name.c_str(), s.c_str()); 89 | p = def; 90 | ret = false; 91 | } 92 | 93 | if (ret && !ros::param::has(r)) { 94 | ROS_DEBUG("%s: Missing required parameter '%s', using default.", 95 | name.c_str(), s.c_str()); 96 | p = def; 97 | ret = false; 98 | } 99 | 100 | if (ret && !ros::param::get(r, p)) { 101 | ROS_DEBUG("%s: Failed to get parameter '%s', using default.", name.c_str(), 102 | s.c_str()); 103 | p = def; 104 | ret = false; 105 | } 106 | 107 | return ret; 108 | } 109 | } 110 | #endif 111 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Matrix2x2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIX2X2_H 2 | #define GEOMETRY_UTILS_MATRIX2X2_H 3 | 4 | #include "MatrixNxNBase.h" 5 | #include "Vector2.h" 6 | 7 | namespace geometry_utils { 8 | 9 | template 10 | struct Matrix2x2Base : MatrixNxNBase { 11 | Matrix2x2Base() : MatrixNxNBase() {} 12 | Matrix2x2Base(T val) : MatrixNxNBase(val) {} 13 | Matrix2x2Base(const Matrix2x2Base& in) : MatrixNxNBase(in.data) {} 14 | Matrix2x2Base(const boost::array& in) : MatrixNxNBase(in) {} 15 | Matrix2x2Base(T (&in)[2 * 2]) : MatrixNxNBase(in) {} 16 | Matrix2x2Base(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 17 | Matrix2x2Base(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 18 | Matrix2x2Base(const MatrixNxMBase& in) : MatrixNxNBase(in) {} 19 | 20 | Matrix2x2Base(T R11, T R12, T R21, T R22) { 21 | this->data[0] = R11; 22 | this->data[1] = R12; 23 | this->data[2] = R21; 24 | this->data[3] = R22; 25 | } 26 | 27 | virtual inline T Det() const { 28 | T a = this->data[0]; 29 | T b = this->data[1]; 30 | T c = this->data[2]; 31 | T d = this->data[3]; 32 | return (-(b * c) + a * d); 33 | } 34 | 35 | virtual inline MatrixNxNBase Inv() const { 36 | Vector2Base e(SingularValues()); 37 | 38 | T emax = e(0); 39 | T emin = e(1); 40 | 41 | if (emin < std::numeric_limits::denorm_min()) 42 | throw std::runtime_error("Matrix2x2Base: appears singular"); 43 | 44 | if (emax / emin > std::numeric_limits::epsilon()) { 45 | T a = this->data[0]; 46 | T b = this->data[1]; 47 | T c = this->data[2]; 48 | T d = this->data[3]; 49 | 50 | T tmp[4] = {d / (-b * c + a * d), b / (b * c - a * d), 51 | c / (b * c - a * d), a / (-b * c + a * d)}; 52 | return MatrixNxNBase(tmp); 53 | } else 54 | throw std::runtime_error("Matrix2x2Base: appears singular"); 55 | } 56 | 57 | virtual inline Vector2Base SingularValues() const { 58 | T a = this->data[0]; 59 | T b = this->data[1]; 60 | T c = this->data[2]; 61 | T d = this->data[3]; 62 | 63 | T tmp1 = a * a + b * b + c * c + d * d; 64 | T tmp2 = math::sqrt((math::pow(b + c, static_cast(2)) + 65 | math::pow(a - d, static_cast(2))) * 66 | (math::pow(b - c, static_cast(2)) + 67 | math::pow(a + d, static_cast(2)))); 68 | 69 | T e1 = math::sqrt(tmp1 - tmp2) * M_SQRT1_2; 70 | T e2 = math::sqrt(tmp1 + tmp2) * M_SQRT1_2; 71 | 72 | return Vector2Base(e1 > e2 ? e1 : e2, e1 < e2 ? e1 : e2); 73 | } 74 | }; 75 | 76 | inline Matrix2x2Base operator*(const float& lhs, 77 | const Matrix2x2Base& rhs) { 78 | return Matrix2x2Base(rhs * lhs); 79 | } 80 | 81 | inline Matrix2x2Base operator*(const double& lhs, 82 | const Matrix2x2Base& rhs) { 83 | return Matrix2x2Base(rhs * lhs); 84 | } 85 | 86 | typedef Matrix2x2Base Matrix2x2f; 87 | typedef Matrix2x2Base Mat22f; 88 | 89 | typedef Matrix2x2Base Matrix2x2d; 90 | typedef Matrix2x2Base Mat22d; 91 | 92 | typedef Matrix2x2Base Matrix2x2; 93 | typedef Matrix2x2Base Mat22; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Transform2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_TRANSFORM2_H 2 | #define GEOMETRY_UTILS_TRANSFORM2_H 3 | 4 | #include "Vector2.h" 5 | #include "Rotation2.h" 6 | 7 | namespace geometry_utils { 8 | 9 | template 10 | struct Transform2Base { 11 | typedef boost::shared_ptr Ptr; 12 | typedef boost::shared_ptr ConstPtr; 13 | 14 | Vector2Base translation; 15 | Rotation2Base rotation; 16 | 17 | Transform2Base() { 18 | translation.Zeros(); 19 | rotation.Eye(); 20 | } 21 | 22 | Transform2Base(const Vector2Base& translation_, 23 | const Rotation2Base& rotation_) 24 | : translation(translation_), rotation(rotation_) {} 25 | 26 | Transform2Base(const Transform2Base& in) 27 | : translation(in.translation), rotation(in.rotation) {} 28 | 29 | Transform2Base(T x, T y, T th) : translation(x, y), rotation(th) {} 30 | 31 | Transform2Base& operator=(const Transform2Base& rhs) { 32 | if (this == &rhs) 33 | return *this; 34 | translation = rhs.translation; 35 | rotation = rhs.rotation; 36 | return *this; 37 | } 38 | 39 | Vector2Base operator*(const Vector2Base& p) const { 40 | return rotation * p + translation; 41 | } 42 | 43 | Transform2Base operator+(const Transform2Base& t) const { 44 | return Transform2Base(translation + rotation * t.translation, 45 | rotation * t.rotation); 46 | } 47 | 48 | bool operator==(const Transform2Base& that) const { 49 | return this->Equals(that); 50 | } 51 | 52 | bool operator!=(const Transform2Base& that) const { 53 | return !this->Equals(that); 54 | } 55 | 56 | bool Equals(const Transform2Base& that, const T ptol = 1e-5, 57 | const T rtol = 1e-5) const { 58 | return (translation.Equals(that.translation, ptol) && 59 | rotation.Equals(that.rotation, rtol)); 60 | } 61 | 62 | void Print(const std::string& prefix = std::string()) const { 63 | if (!prefix.empty()) 64 | std::cout << prefix << std::endl; 65 | std::cout << (*this) << std::endl; 66 | } 67 | 68 | static Transform2Base Identity() { 69 | return Transform2Base(); 70 | } 71 | }; 72 | 73 | template 74 | std::ostream& operator<<(std::ostream& out, const Transform2Base& m) { 75 | out << "Translation:" << std::endl << m.translation << std::endl; 76 | out << "Rotation:" << std::endl << m.rotation; 77 | return out; 78 | } 79 | 80 | template 81 | Transform2Base PoseUpdate(const Transform2Base& t1, 82 | const Transform2Base& t2) { 83 | return Transform2Base(t1.translation + t1.rotation * t2.translation, 84 | t1.rotation * t2.rotation); 85 | } 86 | 87 | template 88 | Transform2Base PoseInverse(const Transform2Base& t) { 89 | return Transform2Base(-1.0 * t.rotation.Trans() * t.translation, 90 | t.rotation.Trans()); 91 | } 92 | 93 | template 94 | Transform2Base PoseDelta(const Transform2Base& t1, 95 | const Transform2Base& t2) { 96 | return Transform2Base( 97 | t1.rotation.Trans() * (t2.translation - t1.translation), 98 | t1.rotation.Trans() * t2.rotation); 99 | } 100 | 101 | typedef Transform2Base Transform2f; 102 | typedef Transform2Base Transform2d; 103 | typedef Transform2d Transform2; 104 | typedef Transform2 Tr2; 105 | 106 | } 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /imu_filter/src/imu_filter.cpp: -------------------------------------------------------------------------------- 1 | /* @file imu_filter.cpp 2 | * @brief IMU Filter for ADIS 3 | * @author Cagri Kilic 4 | */ 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | class ImuFilter { 11 | public: 12 | 13 | // IMU data RAW vector for values reading 14 | std::vector imu_raw_ = {0.0,0.0,0.0,0.0,0.0,0.0}; 15 | 16 | // IMU filtered data vector 17 | std::vector imu_filt_ = {0.0,0.0,0.0,0.0,0.0,0.0}; 18 | 19 | // Filter Weight Parameter 20 | int count=0; 21 | 22 | // Output Publisher 23 | ros::Publisher imu_filt_pub_; 24 | 25 | // IMU RAW data callback function 26 | void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); 27 | 28 | // Filter Function 29 | void filter(); 30 | 31 | // Publisher 32 | void publishFilt(const sensor_msgs::Imu::ConstPtr& msg); 33 | 34 | }; 35 | 36 | // Callback for Filtering 37 | void ImuFilter::imuCallback(const sensor_msgs::Imu::ConstPtr &msg) { 38 | 39 | count++; 40 | // Clear IMU Raw Container 41 | 42 | 43 | // Fill IMU Raw Acceleration 44 | imu_raw_[0] += msg->linear_acceleration.x; 45 | imu_raw_[1] += msg->linear_acceleration.y; 46 | imu_raw_[2] += msg->linear_acceleration.z; 47 | 48 | // Fill IMU Raw Angular Velocity 49 | imu_raw_[3] += msg->angular_velocity.x; 50 | imu_raw_[4] += msg->angular_velocity.y; 51 | imu_raw_[5] += msg->angular_velocity.z; 52 | 53 | imu_filt_[0] = imu_raw_[0] / count; 54 | imu_filt_[1] = imu_raw_[1] / count; 55 | imu_filt_[2] = imu_raw_[2] / count; 56 | 57 | 58 | imu_filt_[3] = imu_raw_[3] / count; 59 | imu_filt_[4] = imu_raw_[4] / count; 60 | imu_filt_[5] = imu_raw_[5] / count; 61 | 62 | if (count == 5) { 63 | 64 | // imu_filt_[0] = imu_raw_[0] / 5; 65 | // imu_filt_[1] = imu_raw_[1] / 5; 66 | // imu_filt_[2] = imu_raw_[2] / 5; 67 | // 68 | 69 | // imu_filt_[3] = imu_raw_[3] / 5; 70 | // imu_filt_[4] = imu_raw_[4] / 5; 71 | // imu_filt_[5] = imu_raw_[5] / 5; 72 | // imu_raw_.clear(); 73 | publishFilt(msg); 74 | imu_raw_.clear(); 75 | imu_filt_.clear(); 76 | imu_raw_[0] = 0; 77 | imu_raw_[1] = 0; 78 | imu_raw_[2] = 0; 79 | imu_raw_[3] = 0; 80 | imu_raw_[4] = 0; 81 | imu_raw_[5] = 0; 82 | imu_filt_[0] = 0; 83 | imu_filt_[1] = 0; 84 | imu_filt_[2] = 0; 85 | imu_filt_[3] = 0; 86 | imu_filt_[4] = 0; 87 | imu_filt_[5] = 0; 88 | count = 0; 89 | } 90 | } 91 | 92 | 93 | 94 | // Publish Filtered Message 95 | void ImuFilter::publishFilt(const sensor_msgs::Imu::ConstPtr& msg) { 96 | 97 | sensor_msgs::Imu msg_out; 98 | 99 | msg_out.header.stamp = msg->header.stamp; 100 | msg_out.header.frame_id ="imu_link"; 101 | 102 | msg_out.linear_acceleration.x = imu_filt_[0]; 103 | msg_out.linear_acceleration.y = imu_filt_[1]; 104 | msg_out.linear_acceleration.z = imu_filt_[2]; 105 | 106 | msg_out.angular_velocity.x = imu_filt_[3]; 107 | msg_out.angular_velocity.y = imu_filt_[4]; 108 | msg_out.angular_velocity.z = imu_filt_[5]; 109 | 110 | 111 | imu_filt_pub_.publish(msg_out); 112 | 113 | } 114 | 115 | 116 | int main(int argc, char **argv){ 117 | 118 | // Init ROS 119 | ros::init(argc, argv, "imu_filter"); 120 | 121 | ros::NodeHandle nh; 122 | 123 | ImuFilter imu_filter_; 124 | 125 | message_filters::Subscriber imu_raw_sub(nh,"imu_raw", 1); 126 | imu_raw_sub.registerCallback(boost::bind(&ImuFilter::imuCallback, &imu_filter_,_1)); 127 | 128 | imu_filter_.imu_filt_pub_ = nh.advertise("imu_filtered", 1);; 129 | 130 | 131 | // Spin 132 | ros::spin(); 133 | } 134 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsMath.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2014 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_MATH_H 22 | #define GEOMETRY_UTILS_MATH_H 23 | 24 | #include 25 | 26 | namespace geometry_utils { 27 | namespace math { 28 | 29 | template inline T cos(T in); 30 | template<> inline float cos(float in) { return ::cosf(in); } 31 | template<> inline double cos(double in) { return ::cos(in); } 32 | 33 | template inline T acos(T in); 34 | template<> inline float acos(float in) { return ::acosf(in); } 35 | template<> inline double acos(double in) { return ::acos(in); } 36 | 37 | template inline T sin(T in); 38 | template<> inline float sin(float in) { return ::sinf(in); } 39 | template<> inline double sin(double in) { return ::sin(in); } 40 | 41 | template inline T asin(T in); 42 | template<> inline float asin(float in) { return ::asinf(in); } 43 | template<> inline double asin(double in) { return ::asin(in); } 44 | 45 | template inline T tan(T in); 46 | template<> inline float tan(float in) { return ::tanf(in); } 47 | template<> inline double tan(double in) { return ::tan(in); } 48 | 49 | template inline T atan(T in); 50 | template<> inline float atan(float in) { return ::atanf(in); } 51 | template<> inline double atan(double in) { return ::atan(in); } 52 | 53 | template inline T fabs(T in); 54 | template<> inline float fabs(float in) { return ::fabsf(in); } 55 | template<> inline double fabs(double in) { return ::fabs(in); } 56 | 57 | template inline T fmin(T v1, T v2); 58 | template<> inline float fmin(float v1, float v2) { return ::fminf(v1, v2); } 59 | template<> inline double fmin(double v1, double v2) { return ::fmin(v1, v2); } 60 | 61 | template inline T fmax(T v1, T v2); 62 | template<> inline float fmax(float v1, float v2) { return ::fmaxf(v1, v2); } 63 | template<> inline double fmax(double v1, double v2) { return ::fmax(v1, v2); } 64 | 65 | template inline T sqrt(T in); 66 | template<> inline float sqrt(float in) { return ::sqrtf(in); } 67 | template<> inline double sqrt(double in) { return ::sqrt(in); } 68 | 69 | template inline T pow(T in, T exp); 70 | template<> inline float pow(float in, float exp) { return ::powf(in, exp); } 71 | template<> inline double pow(double in, double exp) { return ::pow(in, exp); } 72 | 73 | template inline T atan2(T y, T x); 74 | template<> inline float atan2(float y, float x) { return ::atan2f(y, x); } 75 | template<> inline double atan2(double y, double x) { return ::atan2(y, x); } 76 | 77 | template inline T hypot(T x, T y); 78 | template<> inline float hypot(float x, float y) { return ::hypotf(x, y); } 79 | template<> inline double hypot(double x, double y) { return ::hypot(x, y); } 80 | 81 | } 82 | } 83 | #endif 84 | -------------------------------------------------------------------------------- /core_navigation/script/utils/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from sklearn.linear_model import LinearRegression 3 | from sklearn.preprocessing import MinMaxScaler 4 | 5 | 6 | def scale(x_train, y_train, x_test, y_test): 7 | scaler = MinMaxScaler(feature_range=(-1, 1)) 8 | 9 | scale_values = np.concatenate((x_train.reshape(-1), y_train.reshape(-1))) 10 | scale_values = np.unique(scale_values) 11 | scaler.fit(scale_values.reshape(-1, 1)) 12 | 13 | x_train = scaler.transform(x_train.reshape(-1, 1)).reshape(x_train.shape[0], x_train.shape[1], x_train.shape[2]) 14 | y_train = scaler.transform(y_train.reshape(-1, 1)).reshape(y_train.shape[0], y_train.shape[1]) 15 | 16 | x_test = scaler.transform(x_test.reshape(-1, 1)).reshape(x_test.shape[0], x_test.shape[1], x_test.shape[2]) 17 | y_test = scaler.transform(y_test.reshape(-1, 1)).reshape(y_test.shape[0], y_test.shape[1]) 18 | 19 | return scaler, x_train, y_train, x_test, y_test 20 | 21 | 22 | def detrend(data, train_percent, window_length): 23 | idx = int(len(data) * train_percent) - (window_length - 1) 24 | 25 | model = LinearRegression() 26 | model.fit(np.arange(idx).reshape(-1, 1), data[:idx]) 27 | 28 | trend = model.predict(np.arange(len(data)).reshape(-1, 1)) 29 | 30 | data = data - trend 31 | 32 | return data, trend 33 | 34 | 35 | def create_window_data(data, window_size): 36 | temp = [] 37 | 38 | for i in range(len(data) - window_size): 39 | temp.append(data[i: i + window_size]) 40 | 41 | temp = np.array(temp) 42 | 43 | x = temp[:, :-1] 44 | y = temp[:, -1] 45 | 46 | x = np.expand_dims(x, axis=2) 47 | y = np.expand_dims(y, axis=1) 48 | 49 | return x, y 50 | 51 | 52 | def create_direct_data(data, window_length, train_percent): 53 | data, trend = detrend(data, train_percent, window_length) 54 | 55 | x, y = create_window_data(data, window_length + 1) 56 | split = int(train_percent * len(x)) 57 | 58 | x_train, y_train = x[:split], y[:split] 59 | x_test, y_test = x[split:], y[split:] 60 | 61 | scaler, x_train, y_train, x_test, y_test = scale(x_train, y_train, x_test, y_test) 62 | 63 | return x_train, y_train, x_test, y_test, scaler, trend 64 | 65 | 66 | def predict_sliding(model, x_train, test_len, seq_len, window_length, full=True): 67 | train_pred = model.predict(x_train) 68 | cur_window = train_pred[-seq_len:] 69 | cur_window = cur_window.reshape(window_length) 70 | pred = [] 71 | 72 | for i in range(test_len): 73 | pred.append(model.predict(np.expand_dims(cur_window, axis=0))[0][0]) 74 | cur_window = cur_window[1:] 75 | cur_window = np.concatenate((cur_window, np.array([pred[i]]))) 76 | 77 | pred = np.array(pred) 78 | 79 | if full: 80 | result = np.concatenate((train_pred.reshape(-1), pred)) 81 | else: 82 | result = pred 83 | 84 | return result 85 | 86 | 87 | def predict_sliding_rnn(model, x_train, test_len, seq_len, window_length, full=True): 88 | train_pred = model.predict(x_train) 89 | cur_window = train_pred[-seq_len:] 90 | cur_window = cur_window.reshape(window_length) 91 | pred = [] 92 | 93 | for i in range(test_len): 94 | pred.append(model.predict(np.expand_dims(cur_window, axis=0))[0][0]) 95 | cur_window = cur_window[1:] 96 | cur_window = np.concatenate((cur_window, np.array([pred[i]]))) 97 | 98 | pred = np.array(pred) 99 | 100 | if full: 101 | result = np.concatenate((train_pred.reshape(-1), pred)) 102 | else: 103 | result = pred 104 | 105 | return result 106 | 107 | 108 | def invert_scale(scaler, values): 109 | if values.ndim == 1: 110 | values = np.reshape(values, (-1, 1)) 111 | 112 | inverted = scaler.inverse_transform(values) 113 | inverted = np.reshape(inverted, -1) 114 | 115 | return inverted 116 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/src/hw_interface_plugin_roboteq_brushed.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #include 37 | 38 | hw_interface_plugin_roboteq::brushed::brushed() 39 | { 40 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) 41 | { 42 | ros::console::notifyLoggerLevelsChanged(); 43 | } 44 | } 45 | 46 | bool hw_interface_plugin_roboteq::brushed::implStart() 47 | { 48 | ROS_INFO("%s:: Plugin start!", pluginName.c_str()); 49 | 50 | /* 51 | * Roboteq usage 52 | * 1. Disable Command Echo 53 | * 2. send '# C' to clear history buffer 54 | * 3. send '?AIC' read analog sensor after conversion 55 | * 4. send '# 20' to have runtime queries repeated at 20 ms delta (50 hz) 56 | */ 57 | std::string initializationCmd = ""; 58 | int initCmdCycle = 0; 59 | if(ros::param::get(pluginName+"/initializationCmd", initializationCmd)) 60 | { 61 | if (!ros::param::get(pluginName+"/initCmdCycle", initCmdCycle)) 62 | { 63 | initCmdCycle = 20; 64 | } 65 | initializationCmd = hw_interface_plugin_roboteq::roboteq_serial::getInitCommands(initializationCmd, initCmdCycle); 66 | } 67 | else 68 | { 69 | ROS_WARN("Roboteq Initialization Command Unspecified, defaulting"); 70 | initializationCmd = "\r^ECHOF 1\r# C\r?AIC\r# 20\r"; 71 | } 72 | ROS_INFO("Roboteq Init Cmd %s", initializationCmd.c_str()); 73 | postInterfaceWriteRequest(hw_interface_support_types::shared_const_buffer(initializationCmd)); 74 | 75 | return true; 76 | } 77 | 78 | bool hw_interface_plugin_roboteq::brushed::implStop() 79 | { 80 | ROS_INFO("%s:: Plugin stop!", pluginName.c_str()); 81 | return true; 82 | } 83 | 84 | bool hw_interface_plugin_roboteq::brushed::implDataHandler() 85 | { 86 | ROS_DEBUG("%s :: Roboteq Brushed Implementation Data Handler", pluginName.c_str()); 87 | //should check size of buffer is equal to size of msg, just in case. 88 | 89 | return true; 90 | } 91 | -------------------------------------------------------------------------------- /hw_interface/include/hw_interface/hw_interface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef HW_INTERFACE_HPP__ 37 | #define HW_INTERFACE_HPP__ 38 | 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #define NUM_THREADS_PER_PLUGIN 2 53 | 54 | namespace interface_worker 55 | { 56 | void worker(boost::shared_ptr ioService); 57 | }; 58 | 59 | class hw_interface { 60 | //friend class interface_worker; 61 | 62 | private: 63 | ros::NodeHandlePtr node; 64 | 65 | boost::shared_ptr interfaceService; 66 | boost::shared_ptr interfaceWork; 67 | 68 | std::vector > interfacePluginVector; 69 | 70 | boost::thread_group interfaceWorkerGroup; 71 | 72 | pluginlib::ClassLoader pluginLoader; 73 | 74 | bool initPlugin(boost::shared_ptr pluginPtr, 75 | std::string pluginName); 76 | 77 | void initInterfacePlugins(); 78 | void initThreadPool(); 79 | 80 | public: 81 | 82 | 83 | hw_interface(); 84 | hw_interface(ros::NodeHandlePtr nhArg); 85 | //hw_interface(int maxNumOfThreads); 86 | 87 | virtual ~hw_interface(); 88 | 89 | void addInterfacePlugins(); 90 | 91 | bool startInterfaces(); 92 | bool stopInterfaces(); //once node has stopped, reset interfaceWork ptr. 93 | 94 | 95 | 96 | 97 | }; 98 | 99 | 100 | #endif //HW_INTERFACE_HPP__ 101 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Transform3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_TRANSFORM3_H 2 | #define GEOMETRY_UTILS_TRANSFORM3_H 3 | 4 | #include "Vector3.h" 5 | #include "Rotation3.h" 6 | #include "Transform2.h" 7 | 8 | namespace geometry_utils { 9 | 10 | template 11 | struct Transform3Base { 12 | typedef boost::shared_ptr Ptr; 13 | typedef boost::shared_ptr ConstPtr; 14 | 15 | Vector3Base translation; 16 | Rotation3Base rotation; 17 | 18 | Transform3Base() { 19 | translation.Zeros(); 20 | rotation.Eye(); 21 | } 22 | 23 | Transform3Base(const Vector3Base& translation_, 24 | const Rotation3Base& rotation_) 25 | : translation(translation_), rotation(rotation_) {} 26 | 27 | Transform3Base(const Transform3Base& in) 28 | : translation(in.translation), rotation(in.rotation) {} 29 | 30 | Transform3Base(const Transform2Base& in) { 31 | translation(0) = in.translation(0); 32 | translation(1) = in.translation(1); 33 | translation(2) = 0; 34 | rotation.Eye(); 35 | for (unsigned int i = 0; i < 2; i++) 36 | for (unsigned int j = 0; j < 2; j++) rotation(i, j) = in.Rotation(i, j); 37 | } 38 | 39 | Transform3Base& operator=(const Transform3Base& rhs) { 40 | if (this == &rhs) 41 | return *this; 42 | translation = rhs.translation; 43 | rotation = rhs.rotation; 44 | return *this; 45 | } 46 | 47 | Vector3Base operator*(const Vector3Base& p) const { 48 | return rotation * p + translation; 49 | } 50 | 51 | Transform3Base operator+(const Transform3Base& t) const { 52 | return Transform3Base(translation + rotation * t.translation, 53 | rotation * t.rotation); 54 | } 55 | 56 | bool operator==(const Transform3Base& that) const { 57 | return this->Equals(that); 58 | } 59 | 60 | bool operator!=(const Transform3Base& that) const { 61 | return !this->Equals(that); 62 | } 63 | 64 | bool Equals(const Transform3Base& that, const T ptol = 1e-5, 65 | const T rtol = 1e-5) const { 66 | return (translation.Equals(that.translation, ptol) && 67 | rotation.Equals(that.rotation, rtol)); 68 | } 69 | 70 | void Print(const std::string& prefix = std::string()) const { 71 | if (!prefix.empty()) 72 | std::cout << prefix << std::endl; 73 | std::cout << (*this) << std::endl; 74 | } 75 | 76 | static Transform3Base Identity() { 77 | return Transform3Base(); 78 | } 79 | }; 80 | 81 | template 82 | std::ostream& operator<<(std::ostream& out, const Transform3Base& m) { 83 | out << "Translation:" << std::endl << m.translation << std::endl; 84 | out << "Rotation:" << std::endl << m.rotation; 85 | return out; 86 | } 87 | 88 | template 89 | Transform3Base PoseUpdate(const Transform3Base& t1, 90 | const Transform3Base& t2) { 91 | return Transform3Base(t1.translation + t1.rotation * t2.translation, 92 | t1.rotation * t2.rotation); 93 | } 94 | 95 | template 96 | Transform3Base PoseInverse(const Transform3Base& t) { 97 | return Transform3Base(-1.0 * t.rotation.Trans() * t.translation, 98 | t.rotation.Trans()); 99 | } 100 | 101 | template 102 | Transform3Base PoseDelta(const Transform3Base& t1, 103 | const Transform3Base& t2) { 104 | return Transform3Base( 105 | t1.rotation.Trans() * (t2.translation - t1.translation), 106 | t1.rotation.Trans() * t2.rotation); 107 | } 108 | 109 | typedef Transform3Base Transform3f; 110 | typedef Transform3Base Transform3d; 111 | typedef Transform3d Transform3; 112 | typedef Transform3 Tr3; 113 | 114 | } 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /pathfinder_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pathfinder_control 4 | 0.0.0 5 | The pathfinder_control package 6 | 7 | WVU Interactive Robotics Laboratory 8 | Nicholas Ohi/nohi@mix.wvu.edu 9 | https://github.com/wvu-irl/fast-traverse-pathfinder 10 | https://web.statler.wvu.edu/~irl/ 11 | 12 | BSD 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 | catkin 48 | joy 49 | roscpp 50 | rospy 51 | teleop_twist_joy 52 | twist_mux 53 | controller_manager 54 | joint_state_controller 55 | robot_state_publisher 56 | tf 57 | nav_msgs 58 | joy 59 | roscpp 60 | rospy 61 | teleop_twist_joy 62 | twist_mux 63 | controller_manager 64 | joint_state_controller 65 | robot_state_publisher 66 | tf 67 | nav_msgs 68 | joy 69 | roscpp 70 | rospy 71 | teleop_twist_joy 72 | twist_mux 73 | controller_manager 74 | joint_state_controller 75 | robot_state_publisher 76 | tf 77 | nav_msgs 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2014 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_H 22 | #define GEOMETRY_UTILS_H 23 | 24 | #include "Vector2.h" 25 | #include "Vector3.h" 26 | #include "Vector4.h" 27 | #include "Quaternion.h" 28 | #include "Matrix2x2.h" 29 | #include "Matrix3x3.h" 30 | #include "Matrix4x4.h" 31 | #include "Rotation2.h" 32 | #include "Rotation3.h" 33 | #include "Transform2.h" 34 | #include "Transform3.h" 35 | 36 | namespace geometry_utils { 37 | inline double Unroll(double x) { 38 | x = fmod(x, 2.0 * M_PI); 39 | if (x < 0) 40 | x += 2.0 * M_PI; 41 | return x; 42 | } 43 | 44 | inline double Normalize(double x) { 45 | x = fmod(x + M_PI, 2.0 * M_PI); 46 | if (x < 0) 47 | x += 2.0 * M_PI; 48 | return x - M_PI; 49 | } 50 | 51 | inline double S1Distance(double from, double to) { 52 | double result = Unroll(Unroll(to) - Unroll(from)); 53 | if (result > M_PI) 54 | result = -(2.0 * M_PI - result); 55 | return Normalize(result); 56 | } 57 | 58 | inline double Rad2Deg(double angle) { 59 | return angle * 180.0 * M_1_PI; 60 | } 61 | 62 | inline double Deg2Rad(double angle) { 63 | return angle * M_PI / 180.0; 64 | } 65 | 66 | inline Vec3 Rad2Deg(const Vec3& angles) { 67 | return Vec3(Rad2Deg(angles(0)), Rad2Deg(angles(1)), Rad2Deg(angles(2))); 68 | } 69 | 70 | inline Vec3 Deg2Rad(const Vec3& angles) { 71 | return Vec3(Deg2Rad(angles(0)), Deg2Rad(angles(1)), Deg2Rad(angles(2))); 72 | } 73 | 74 | inline Vec3 RToZYX(const Rot3& rot) { 75 | return rot.GetEulerZYX(); 76 | } 77 | 78 | inline Rot3 ZYXToR(const Vec3& angles) { 79 | return Rot3(angles); 80 | } 81 | 82 | inline Rot3 QuatToR(const Quat& quat) { 83 | return Rot3(quat); 84 | } 85 | 86 | inline Quat RToQuat(const Rot3& rot) { 87 | return Quat(Eigen::Quaterniond(rot.Eigen())); 88 | } 89 | 90 | inline double GetRoll(const Rot3& r) { 91 | return r.Roll(); 92 | } 93 | 94 | inline double GetRoll(const Quat& q) { 95 | return Rot3(q).Roll(); 96 | } 97 | 98 | inline double GetPitch(const Rot3& r) { 99 | return r.Pitch(); 100 | } 101 | 102 | inline double GetPitch(const Quat& q) { 103 | return Rot3(q).Pitch(); 104 | } 105 | 106 | inline double GetYaw(const Rot3& r) { 107 | return r.Yaw(); 108 | } 109 | 110 | inline double GetYaw(const Quat& q) { 111 | return Rot3(q).Yaw(); 112 | } 113 | 114 | inline double SO3Error(const Quat& q1, const Quat& q2) { 115 | return Rot3(q1).Error(Rot3(q2)); 116 | } 117 | 118 | inline double SO3Error(const Rot3& r1, const Rot3& r2) { 119 | return r1.Error(r2); 120 | } 121 | 122 | inline Vec3 CartesianToSpherical(const Vec3& v) { 123 | double rho = v.Norm(); 124 | return Vec3(rho, acos(v.Z() / rho), atan2(v.Y(), v.X())); 125 | } 126 | 127 | inline Vec3 SphericalToCartesian(const Vec3& v) { 128 | return Vec3(v(0) * sin(v(1)) * cos(v(2)), v(0) * sin(v(1)) * sin(v(2)), 129 | v(0) * cos(v(1))); 130 | } 131 | 132 | inline Vec3 NEDCartesian(const Vec3& v) { 133 | return Vec3(v(0), -v(1), -v(2)); 134 | } 135 | 136 | } 137 | 138 | #endif 139 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/src/hw_interface_plugin_roboteq_brushless.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #include 37 | 38 | hw_interface_plugin_roboteq::brushless::brushless() 39 | { 40 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) 41 | { 42 | ros::console::notifyLoggerLevelsChanged(); 43 | } 44 | } 45 | 46 | bool hw_interface_plugin_roboteq::brushless::implStart() 47 | { 48 | ROS_INFO("%s:: Plugin start!", pluginName.c_str()); 49 | 50 | /* 51 | // TODO: rmc stuff - most likely remove 52 | std::string wheelType; 53 | ros::param::get(pluginName+"/wheelType", wheelType); 54 | 55 | postInterfaceWriteRequest(hw_interface_support_types::shared_const_buffer(script_list[wheelType])); 56 | */ 57 | 58 | /* 59 | * Roboteq usage 60 | * 1. send '# C' to clear history buffer 61 | * 2. send '?CB' to get absolute brushless encoder count 62 | * 3. send '# 20' to have runtime queries repeated at 20 ms delta (50 hz) 63 | */ 64 | std::string initializationCmd = ""; 65 | int initCmdCycle = 0; 66 | if(ros::param::get(pluginName+"/initializationCmd", initializationCmd)) 67 | { 68 | if (!ros::param::get(pluginName+"/initCmdCycle", initCmdCycle)) 69 | { 70 | initCmdCycle = 20; 71 | } 72 | initializationCmd = hw_interface_plugin_roboteq::roboteq_serial::getInitCommands(initializationCmd, initCmdCycle); 73 | } 74 | else 75 | { 76 | ROS_WARN("Roboteq Initialization Command Unspecified, defaulting"); 77 | initializationCmd = "\r^ECHOF 1\r# C\r?CB\r# 20\r"; 78 | } 79 | ROS_INFO("Roboteq Init Cmd %s", initializationCmd.c_str()); 80 | postInterfaceWriteRequest(hw_interface_support_types::shared_const_buffer(initializationCmd)); 81 | 82 | return true; 83 | } 84 | 85 | bool hw_interface_plugin_roboteq::brushless::implStop() 86 | { 87 | ROS_INFO("%s:: Plugin stop!", pluginName.c_str()); 88 | return true; 89 | } 90 | 91 | bool hw_interface_plugin_roboteq::brushless::implDataHandler() 92 | { 93 | ROS_DEBUG("%s :: Roboteq Brushless Implementation Data Handler", pluginName.c_str()); 94 | //should check size of buffer is equal to size of msg, just in case. 95 | 96 | return true; 97 | } 98 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Matrix3x3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIX3X3_H 2 | #define GEOMETRY_UTILS_MATRIX3X3_H 3 | 4 | #include "MatrixNxNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Matrix3x3Base : MatrixNxNBase { 10 | Matrix3x3Base() : MatrixNxNBase() {} 11 | Matrix3x3Base(T val) : MatrixNxNBase(val) {} 12 | Matrix3x3Base(const Matrix3x3Base& in) : MatrixNxNBase(in.data) {} 13 | Matrix3x3Base(const boost::array& in) : MatrixNxNBase(in) {} 14 | Matrix3x3Base(T (&in)[9]) : MatrixNxNBase(in) {} 15 | Matrix3x3Base(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 16 | Matrix3x3Base(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 17 | Matrix3x3Base(const MatrixNxMBase& in) : MatrixNxNBase(in) {} 18 | 19 | Matrix3x3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33) { 20 | this->data[0] = R11; 21 | this->data[1] = R12; 22 | this->data[2] = R13; 23 | this->data[3] = R21; 24 | this->data[4] = R22; 25 | this->data[5] = R23; 26 | this->data[6] = R31; 27 | this->data[7] = R32; 28 | this->data[8] = R33; 29 | } 30 | 31 | inline T Det() const { 32 | T a = this->data[0]; 33 | T b = this->data[1]; 34 | T c = this->data[2]; 35 | T d = this->data[3]; 36 | T e = this->data[4]; 37 | T f = this->data[5]; 38 | T g = this->data[6]; 39 | T h = this->data[7]; 40 | T i = this->data[8]; 41 | return (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + 42 | a * e * i); 43 | } 44 | 45 | virtual inline MatrixNxNBase Inv() const { 46 | if (math::fabs(Det()) < std::numeric_limits::denorm_min()) 47 | throw std::runtime_error("Matrix3x3Base: appears singular"); 48 | 49 | T a = this->data[0]; 50 | T b = this->data[1]; 51 | T c = this->data[2]; 52 | T d = this->data[3]; 53 | T e = this->data[4]; 54 | T f = this->data[5]; 55 | T g = this->data[6]; 56 | T h = this->data[7]; 57 | T i = this->data[8]; 58 | T tmp[9] = {(f * h - e * i) / (c * e * g - b * f * g - c * d * h + 59 | a * f * h + b * d * i - a * e * i), 60 | (c * h - b * i) / (-(c * e * g) + b * f * g + c * d * h - 61 | a * f * h - b * d * i + a * e * i), 62 | (c * e - b * f) / (c * e * g - b * f * g - c * d * h + 63 | a * f * h + b * d * i - a * e * i), 64 | (f * g - d * i) / (-(c * e * g) + b * f * g + c * d * h - 65 | a * f * h - b * d * i + a * e * i), 66 | (c * g - a * i) / (c * e * g - b * f * g - c * d * h + 67 | a * f * h + b * d * i - a * e * i), 68 | (c * d - a * f) / (-(c * e * g) + b * f * g + c * d * h - 69 | a * f * h - b * d * i + a * e * i), 70 | (e * g - d * h) / (c * e * g - b * f * g - c * d * h + 71 | a * f * h + b * d * i - a * e * i), 72 | (b * g - a * h) / (-(c * e * g) + b * f * g + c * d * h - 73 | a * f * h - b * d * i + a * e * i), 74 | (b * d - a * e) / (c * e * g - b * f * g - c * d * h + 75 | a * f * h + b * d * i - a * e * i)}; 76 | return MatrixNxNBase(tmp); 77 | } 78 | }; 79 | 80 | inline Matrix3x3Base operator*(const float& lhs, 81 | const Matrix3x3Base& rhs) { 82 | return Matrix3x3Base(rhs.Scale(lhs)); 83 | } 84 | 85 | inline Matrix3x3Base operator*(const double& lhs, 86 | const Matrix3x3Base& rhs) { 87 | return Matrix3x3Base(rhs.Scale(lhs)); 88 | } 89 | 90 | typedef Matrix3x3Base Matrix3x3f; 91 | typedef Matrix3x3Base Mat33f; 92 | 93 | typedef Matrix3x3Base Matrix3x3d; 94 | typedef Matrix3x3Base Mat33d; 95 | 96 | typedef Matrix3x3Base Matrix3x3; 97 | typedef Matrix3x3Base Mat33; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /init_core_navigation/include/init_core_navigation/InitCoreNav.h: -------------------------------------------------------------------------------- 1 | #ifndef init_core_navigation_H 2 | #define init_core_navigation_H 3 | 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 | class InitCoreNav { 18 | public: 19 | 20 | typedef sensor_msgs::Imu ImuData; 21 | typedef geometry_msgs::PoseStamped PoseData; 22 | 23 | typedef Eigen::VectorXd Vector; 24 | typedef Eigen::MatrixXd Matrix; 25 | typedef Eigen::Matrix Vector3; 26 | typedef Eigen::Matrix Vector6; 27 | typedef novatel_gps_msgs::NovatelXYZ GPSECEFData; 28 | typedef novatel_gps_msgs::NovatelPosition GPSLLHData; 29 | 30 | InitCoreNav::Vector6 imu; 31 | InitCoreNav::Vector3 gps_ecef; 32 | InitCoreNav::Vector3 gps_llh; 33 | 34 | InitCoreNav(); 35 | ~InitCoreNav(); 36 | 37 | // Calls LoadParameters and RegisterCallbacks. Fails on failure of either. 38 | bool Initialize(const ros::NodeHandle& n); 39 | 40 | private: 41 | // Node initialization 42 | // bool Init(const ros::NodeHandle& n); 43 | bool LoadParameters(const ros::NodeHandle& n); 44 | bool RegisterCallbacks(const ros::NodeHandle& n); 45 | 46 | // Publish estimated states. 47 | void PublishStates(const InitCoreNav::Vector3& states, const ros::Publisher& pub); 48 | void writeParams(std::string path_to_param_file, const InitCoreNav::Vector3& ins_bias_a, const InitCoreNav::Vector3& ins_bias_g, const InitCoreNav::Vector3& init_ecef, const InitCoreNav::Vector3& init_llh); 49 | void ImuCallback(const ImuData& imu_data_); 50 | void Propagate(const InitCoreNav::Vector6& imu,const InitCoreNav::Vector3& gps_ecef, const InitCoreNav::Vector3& gps_llh); 51 | void GPSLLHCallBack(const GPSLLHData& gps_llh_data_); 52 | void GPSECEFCallBack(const GPSECEFData& gps_ecef_data_); 53 | 54 | 55 | InitCoreNav::Vector6 getImuData(const ImuData& imu_data_); 56 | InitCoreNav::Vector3 getGPSECEFData(const GPSECEFData& gps_ecef_data_); 57 | InitCoreNav::Vector3 getGPSLLHData(const GPSLLHData& gps_llh_data_); 58 | 59 | 60 | // The node's name. 61 | std::string name_; 62 | 63 | // Subscriber 64 | ros::Subscriber imu_sub_; 65 | ros::Subscriber gps_ecef_sub_; 66 | ros::Subscriber gps_llh_sub_; 67 | 68 | // Publisher. 69 | ros::Publisher bias_a_pub_; 70 | ros::Publisher bias_g_pub_, init_llh_pub_, init_ecef_pub_; 71 | tf::TransformBroadcaster transformed_states_tf_broad; 72 | 73 | GPSECEFData gps_ecef_data_; 74 | GPSLLHData gps_llh_data_; 75 | ImuData imu_data_; 76 | 77 | bool has_imu_ = false; 78 | bool first_imu_ = true; 79 | bool has_gpsLLH_=false; 80 | bool has_gpsECEF_=false; 81 | 82 | // Most recent time stamp for publishers. 83 | ros::Time stamp_; 84 | 85 | // Coordinate frames. 86 | std::string frame_id_out_; 87 | std::string frame_id_imu_; 88 | std::string frame_id_fixed_; 89 | 90 | // update rate [hz] 91 | unsigned int publish_hz_; 92 | unsigned int sensor_pub_rate_; 93 | 94 | // sub. topics 95 | std::string imu_topic_; 96 | std::string gps_llh_topic_; 97 | std::string gps_ecef_topic_; 98 | // For initialization. 99 | bool initialized_; 100 | 101 | // Filter vars. 102 | InitCoreNav::Vector3 bias_a_; 103 | InitCoreNav::Vector3 bias_g_; 104 | InitCoreNav::Vector3 ins_bias_a; 105 | InitCoreNav::Vector3 ins_bias_g; 106 | InitCoreNav::Vector3 init_llh; 107 | InitCoreNav::Vector3 init_ecef; 108 | 109 | InitCoreNav::Vector3 omega_b_ib_; 110 | InitCoreNav::Vector3 f_ib_b_; 111 | 112 | // initial pose 113 | 114 | double init_ba_x, init_ba_y, init_ba_z, init_bg_x, init_bg_y, init_bg_z; 115 | double init_ecef_x,init_ecef_y,init_ecef_z; 116 | double init_x, init_y, init_z; 117 | double imu_stamp_curr_, imu_stamp_prev_; 118 | double dt_imu_; 119 | int count=0; 120 | 121 | }; 122 | #endif 123 | -------------------------------------------------------------------------------- /hw_interface/include/hw_interface/base_UDP_interface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef BASE_UDP_INTERFACE_HPP__ 37 | #define BASE_UDP_INTERFACE_HPP__ 38 | 39 | 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #define UDP_FRAME_BUFFER_SIZE 1500 49 | #define UDP_MAX_PKT_SIZE 65500 50 | 51 | namespace base_classes 52 | { 53 | class base_UDP_interface : public base_interface 54 | { 55 | //friend class hw_interface; 56 | 57 | private: 58 | 59 | bool interfaceReady(); 60 | bool initPlugin(ros::NodeHandlePtr nhPtr, 61 | const boost::shared_ptr ioService); 62 | bool startWork(); //probably set some flag and start listening on port 63 | bool stopWork(); //probably unset some flag 64 | 65 | //will receive request, then run plugin readHandler(), check work flag, 66 | //if work flag restart async read 67 | 68 | protected: 69 | 70 | base_UDP_interface(); 71 | 72 | boost::shared_ptr interfaceSocket; 73 | boost::shared_ptr localEndpoint; 74 | boost::shared_ptr remoteEndpoint; 75 | boost::asio::ip::udp::endpoint senderEndpoint; 76 | 77 | //needs better name 78 | //this function calls the plugin's function to read in ROS params, 79 | //subscribe to topics, publish topics. This function should fill 80 | //in the protected member's info 81 | virtual bool subPluginInit(ros::NodeHandlePtr nhPtr) = 0; 82 | 83 | virtual bool pluginStart() 84 | { 85 | return true; 86 | } 87 | 88 | virtual bool pluginStop() 89 | { 90 | return true; 91 | } 92 | 93 | boost::asio::ip::address localAddress; 94 | int localPort; 95 | boost::asio::ip::address remoteAddress; 96 | int remotePort; 97 | 98 | //plugin provided data handler that moves data into ROS 99 | virtual bool interfaceReadHandler(const size_t &bufferSize, int arrayStartPos) = 0; 100 | 101 | void interfaceWriteHandler(const hw_interface_support_types::shared_const_buffer &buffer); 102 | void postInterfaceWriteRequest(const hw_interface_support_types::shared_const_buffer &buffer); 103 | 104 | public: 105 | bool handleIORequest(const boost::system::error_code &ec, size_t bytesReceived); 106 | 107 | }; 108 | }; 109 | #endif //BASE_UDP_INTERFACE_HPP__ 110 | -------------------------------------------------------------------------------- /hw_interface/include/hw_interface/telemetry.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef TELEMETRY_TIME_HPP__ 37 | #define TELEMETRY_TIME_HPP__ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | class delta_loop_time 45 | { 46 | private: 47 | 48 | bool metricsEnabled; 49 | 50 | //boost::mutex metricMuxtex; 51 | //ros::Time does not need a thread lock because it is actually locked internally 52 | boost::accumulators::accumulator_set > acc; 53 | ros::Time lastTimeMetric; 54 | double deltaTime; 55 | 56 | public: 57 | delta_loop_time(): 58 | lastTimeMetric(ros::Time::now()), 59 | acc(boost::accumulators::tag::rolling_window::window_size = 150) 60 | { 61 | metricsEnabled = true; 62 | } 63 | bool enableMetrics() 64 | { 65 | metricsEnabled = true; 66 | this->lastTimeMetric = ros::Time::now(); 67 | return metricsEnabled; 68 | }; 69 | 70 | bool disableMetrics() 71 | { 72 | metricsEnabled = false; 73 | return metricsEnabled; 74 | }; 75 | 76 | float updateMetrics() 77 | { 78 | if(metricsEnabled) 79 | { 80 | boost::scoped_ptr output(new char[255]); 81 | deltaTime = ros::Time::now().toSec()-this->lastTimeMetric.toSec(); 82 | acc(deltaTime); 83 | this->lastTimeMetric = ros::Time::now(); 84 | return 1/(boost::accumulators::rolling_mean(acc)); 85 | } 86 | return 0; 87 | }; 88 | 89 | std::string printMetrics(bool printSideEffect) 90 | { 91 | if(metricsEnabled) 92 | { 93 | boost::scoped_ptr output(new char[255]); 94 | deltaTime = ros::Time::now().toSec()-this->lastTimeMetric.toSec(); 95 | acc(deltaTime); 96 | sprintf(output.get(), "Delta Time %2.3f:: Hz %2.2f:: Avg Hz %2.2f", 97 | deltaTime, 1/deltaTime, 1/(boost::accumulators::rolling_mean(acc))); 98 | this->lastTimeMetric = ros::Time::now(); 99 | if(printSideEffect) 100 | { 101 | ROS_DEBUG("%s", output.get()); 102 | } 103 | 104 | std::string outputString(1, *output); 105 | return outputString; 106 | } 107 | else 108 | { 109 | return ""; 110 | } 111 | }; 112 | }; 113 | 114 | #endif //TELEMETRY_TIME_HPP__ 115 | 116 | -------------------------------------------------------------------------------- /hw_interface/src/base_UDP_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #define THREAD_ID_TO_C_STR boost::lexical_cast(boost::this_thread::get_id()).c_str() 6 | 7 | base_classes::base_UDP_interface::base_UDP_interface() 8 | { 9 | ROS_DEBUG_EXTRA_SINGLE("UDP Plugin initialized"); 10 | interfaceType = base_classes::UDP; 11 | interfaceStarted = false; 12 | 13 | receivedData = boost::shared_array(new uint8_t[UDP_MAX_PKT_SIZE]); 14 | remoteEndpoint = boost::shared_ptr(); 15 | localEndpoint = boost::shared_ptr(); 16 | interfaceSocket = boost::shared_ptr(); 17 | } 18 | 19 | bool base_classes::base_UDP_interface::interfaceReady() 20 | { 21 | if(!interfaceSocket.get()) 22 | { 23 | return false; 24 | } 25 | return interfaceSocket->is_open(); 26 | } 27 | 28 | bool base_classes::base_UDP_interface::initPlugin(ros::NodeHandlePtr nhPtr, 29 | const boost::shared_ptr ioService) 30 | { 31 | enableCompletionFunctor = false; 32 | enableRegexReadUntil = false; 33 | //initilize output data stream strand 34 | interfaceSynchronousStrand = boost::shared_ptr(new boost::asio::strand(*ioService)); 35 | //call plugin setup 36 | ROS_DEBUG_EXTRA_SINGLE("Calling Plugin's Init"); 37 | subPluginInit(nhPtr); 38 | 39 | ROS_DEBUG_EXTRA_SINGLE("Creating Endpoints"); 40 | remoteEndpoint = boost::shared_ptr(new boost::asio::ip::udp::endpoint(remoteAddress, remotePort)); 41 | //create the local endpoint 42 | localEndpoint = boost::shared_ptr(new boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), localPort)); 43 | //open, bind, and connect to the local endpoint 44 | ROS_DEBUG_EXTRA_SINGLE("Opening Local Socket"); 45 | interfaceSocket = boost::shared_ptr(new boost::asio::ip::udp::socket(*ioService, *localEndpoint)); 46 | ROS_DEBUG_EXTRA_SINGLE("Socket Created"); 47 | return interfaceReady(); 48 | } 49 | 50 | bool base_classes::base_UDP_interface::startWork() 51 | { 52 | if(!interfaceReady()) 53 | { 54 | return false; 55 | } 56 | 57 | if(!interfaceStarted) 58 | { 59 | interfaceStarted = pluginStart(); 60 | ROS_INFO("Setting Socket send size to %d", UDP_MAX_PKT_SIZE); 61 | boost::asio::socket_base::send_buffer_size option(UDP_MAX_PKT_SIZE); 62 | interfaceSocket->set_option(option); 63 | } 64 | ROS_DEBUG("Starting UDP Work"); 65 | 66 | interfaceSocket.get()->async_receive_from(boost::asio::buffer(receivedData.get(), UDP_MAX_PKT_SIZE), senderEndpoint, 67 | boost::bind(&base_UDP_interface::handleIORequest, this, 68 | boost::asio::placeholders::error(), 69 | boost::asio::placeholders::bytes_transferred())); 70 | return true; 71 | } 72 | 73 | bool base_classes::base_UDP_interface::stopWork() 74 | { 75 | if(interfaceStarted) 76 | { 77 | //free all queued work 78 | interfaceSocket->cancel(); 79 | //probably shouldn't close 80 | interfaceSocket->close(); 81 | 82 | interfaceStarted = false; 83 | return !interfaceSocket->is_open(); 84 | } 85 | return false; 86 | } 87 | 88 | bool base_classes::base_UDP_interface::handleIORequest(const boost::system::error_code &ec, size_t bytesReceived) 89 | { 90 | //printMetrics(true); 91 | ROS_INFO_THROTTLE(5,"Thread <%s>:: %s:: Received Packet!:: Size %lu", THREAD_ID_TO_C_STR, this->pluginName.c_str(), bytesReceived); 92 | 93 | //call plugin's data handler 94 | dataArrayStart=0; 95 | if(!interfaceReadHandler(bytesReceived, dataArrayStart)) 96 | { 97 | ROS_ERROR("Error Occurred in plugin data Handler <%s>", this->pluginName.c_str()); 98 | } 99 | 100 | //restart the work 101 | return startWork(); 102 | } 103 | 104 | void base_classes::base_UDP_interface::postInterfaceWriteRequest(const hw_interface_support_types::shared_const_buffer &buffer) 105 | { 106 | ROS_DEBUG("%s:: Requesting interface write", pluginName.c_str()); 107 | interfaceSynchronousStrand->post(boost::bind(&base_UDP_interface::interfaceWriteHandler, this, 108 | buffer)); 109 | } 110 | 111 | void base_classes::base_UDP_interface::interfaceWriteHandler(const hw_interface_support_types::shared_const_buffer &buffer) 112 | { 113 | ROS_DEBUG("%s:: Writing Commands to interface", pluginName.c_str()); 114 | try { 115 | //boost::system::system_error e(); 116 | interfaceSocket->send_to(buffer,*remoteEndpoint); 117 | } 118 | catch(boost::system::system_error &ec) 119 | { 120 | ROS_ERROR("%s:: Caught Exception on WRITE!! %s", pluginName.c_str(), ec.what()); 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsROS.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2015 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_ROS_H 22 | #define GEOMETRY_UTILS_ROS_H 23 | 24 | #include "GeometryUtils.h" 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace geometry_utils { 34 | namespace ros { 35 | 36 | inline Vec3 FromROS(const geometry_msgs::Point& p) { 37 | return Vec3(p.x, p.y, p.z); 38 | } 39 | 40 | inline Vec3 FromROS(const geometry_msgs::Point32& p) { 41 | return Vec3(p.x, p.y, p.z); 42 | } 43 | 44 | inline Vec3 FromROS(const geometry_msgs::Vector3& p) { 45 | return Vec3(p.x, p.y, p.z); 46 | } 47 | 48 | inline Quat FromROS(const geometry_msgs::Quaternion& msg) { 49 | return Quat(msg.w, msg.x, msg.y, msg.z); 50 | } 51 | 52 | inline Transform3 FromROS(const geometry_msgs::Pose& msg) { 53 | return Transform3(FromROS(msg.position), QuatToR(FromROS(msg.orientation))); 54 | } 55 | 56 | inline Transform3 FromROS(const geometry_msgs::Transform& msg) { 57 | return Transform3(FromROS(msg.translation), QuatToR(FromROS(msg.rotation))); 58 | } 59 | 60 | inline geometry_msgs::Point ToRosPoint(const Vec2& v) { 61 | geometry_msgs::Point msg; 62 | msg.x = v(0); 63 | msg.y = v(1); 64 | msg.z = 0.0; 65 | 66 | return msg; 67 | } 68 | 69 | inline geometry_msgs::Point ToRosPoint(const Vec3& v) { 70 | geometry_msgs::Point msg; 71 | msg.x = v(0); 72 | msg.y = v(1); 73 | msg.z = v(2); 74 | 75 | return msg; 76 | } 77 | 78 | inline geometry_msgs::Point32 ToRosPoint32(const Vec2& v) { 79 | geometry_msgs::Point32 msg; 80 | msg.x = v(0); 81 | msg.y = v(1); 82 | msg.z = 0.0f; 83 | 84 | return msg; 85 | } 86 | 87 | inline geometry_msgs::Point32 ToRosPoint32(const Vec3& v) { 88 | geometry_msgs::Point32 msg; 89 | msg.x = v(0); 90 | msg.y = v(1); 91 | msg.z = v(2); 92 | 93 | return msg; 94 | } 95 | 96 | inline geometry_msgs::Vector3 ToRosVec(const Vec2& v) { 97 | geometry_msgs::Vector3 msg; 98 | msg.x = v(0); 99 | msg.y = v(1); 100 | msg.z = 0.0; 101 | 102 | return msg; 103 | } 104 | 105 | inline geometry_msgs::Vector3 ToRosVec(const Vec3& v) { 106 | geometry_msgs::Vector3 msg; 107 | msg.x = v(0); 108 | msg.y = v(1); 109 | msg.z = v(2); 110 | 111 | return msg; 112 | } 113 | 114 | inline geometry_msgs::Quaternion ToRosQuat(const Quat& quat) { 115 | geometry_msgs::Quaternion msg; 116 | msg.w = quat.W(); 117 | msg.x = quat.X(); 118 | msg.y = quat.Y(); 119 | msg.z = quat.Z(); 120 | 121 | return msg; 122 | } 123 | 124 | inline geometry_msgs::Pose ToRosPose(const Transform2& trans) { 125 | geometry_msgs::Pose msg; 126 | msg.position = ToRosPoint(trans.translation); 127 | msg.orientation = ToRosQuat(RToQuat(Rot3(trans.rotation))); 128 | 129 | return msg; 130 | } 131 | 132 | inline geometry_msgs::Pose ToRosPose(const Transform3& trans) { 133 | geometry_msgs::Pose msg; 134 | msg.position = ToRosPoint(trans.translation); 135 | msg.orientation = ToRosQuat(RToQuat(trans.rotation)); 136 | 137 | return msg; 138 | } 139 | 140 | inline geometry_msgs::Transform ToRosTransform(const Transform2& trans) { 141 | geometry_msgs::Transform msg; 142 | msg.translation = ToRosVec(trans.translation); 143 | msg.rotation = ToRosQuat(RToQuat(Rot3(trans.rotation))); 144 | 145 | return msg; 146 | } 147 | 148 | inline geometry_msgs::Transform ToRosTransform(const Transform3& trans) { 149 | geometry_msgs::Transform msg; 150 | msg.translation = ToRosVec(trans.translation); 151 | msg.rotation = ToRosQuat(RToQuat(trans.rotation)); 152 | 153 | return msg; 154 | } 155 | 156 | inline geometry_msgs::Quaternion ToRosQuat(const Vec3& angles) { 157 | return ToRosQuat(RToQuat(ZYXToR(angles))); 158 | } 159 | 160 | inline Vec3 RosQuatToZYX(const geometry_msgs::Quaternion& msg) { 161 | return RToZYX(QuatToR(FromROS(msg))); 162 | } 163 | 164 | inline double GetRoll(const geometry_msgs::Quaternion& q) { 165 | return Rot3(FromROS(q)).Roll(); 166 | } 167 | 168 | inline double GetPitch(const geometry_msgs::Quaternion& q) { 169 | return Rot3(FromROS(q)).Pitch(); 170 | } 171 | 172 | inline double GetYaw(const geometry_msgs::Quaternion& q) { 173 | return Rot3(FromROS(q)).Yaw(); 174 | } 175 | 176 | } 177 | } 178 | #endif 179 | -------------------------------------------------------------------------------- /hw_interface_plugins/hw_interface_plugin_roboteq/include/hw_interface_plugin_roboteq/hw_interface_plugin_roboteq.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef HW_INTERFACE_PLUGIN_ROBOTEQ_HPP__ 37 | #define HW_INTERFACE_PLUGIN_ROBOTEQ_HPP__ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include // std::ifstream 52 | 53 | #include 54 | #include 55 | 56 | namespace hw_interface_plugin_roboteq { 57 | 58 | enum controller_t { Other, Left_Drive_Roboteq, Right_Drive_Roboteq }; 59 | 60 | class roboteq_serial : public base_classes::base_serial_interface 61 | { 62 | public: 63 | typedef boost::asio::buffers_iterator matcherIterator; 64 | 65 | roboteq_serial(); 66 | virtual ~roboteq_serial() {} //need to implement closing of the port here 67 | 68 | protected: 69 | ros::NodeHandlePtr nh; 70 | 71 | typedef boost::tokenizer > tokenizer; 72 | std::string getInitCommands(std::string initializationCmd, int initCmdCycle); 73 | 74 | hw_interface_plugin_roboteq::ActuatorOut latestActuatorCmd; 75 | controller_t roboteqType; 76 | 77 | bool subPluginInit(ros::NodeHandlePtr nhPtr); 78 | void setInterfaceOptions(); 79 | bool interfaceReadHandler(const size_t &length, int arrayStartPos, const boost::system::error_code &ec); 80 | bool verifyChecksum(); 81 | 82 | bool pluginStart() 83 | { 84 | return implStart(); 85 | } 86 | 87 | bool pluginStop() 88 | { 89 | return implStop(); 90 | } 91 | 92 | bool implInit(); 93 | void rosMsgCallback(const hw_interface_plugin_roboteq::ActuatorOut::ConstPtr &msgIn); 94 | std::string m_command; 95 | 96 | hw_interface_plugin_roboteq::RoboteqData roboteqData; 97 | 98 | virtual bool implStart() = 0; 99 | virtual bool implStop() = 0; 100 | virtual bool implDataHandler() = 0; 101 | 102 | std::map command_list = { 103 | {"motor_amps", "A"}, 104 | {"analog_inputs", "AI"}, 105 | {"analog_inputs_conversion", "AIC"}, 106 | {"bl_motor_speed_rpm", "BS"}, 107 | {"individual_digital_inputs", "DI"}, 108 | {"destination_reached", "DR"}, 109 | {"fault_flags", "FF"}, 110 | {"encoder_counter_absolute", "C"}, 111 | {"encoder_count_relative", "CR"}, 112 | {"battery_amps", "BA"}, 113 | {"feedback", "F"}, 114 | {"var_2", "VAR"}, 115 | {"volts", "V"} 116 | }; 117 | 118 | std::pair matchFooter(matcherIterator begin, matcherIterator end, const char *sequence); 119 | 120 | std::string roboteqInit; 121 | private: 122 | int m_numInitCmds; // # of commands from launch file 123 | int m_numCmdsMatched; // # of commands matched regex from roboteqs 124 | 125 | bool m_exStop; 126 | 127 | bool dataHandler(tokenizer::iterator tok_iter, tokenizer tokens); 128 | 129 | }; 130 | } 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /hw_interface/include/hw_interface/shared_const_buffer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, WVU Interactive Robotics Laboratory 5 | * https://web.statler.wvu.edu/~irl/ 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the WVU Interactive Robotics Laboratory nor 19 | * the names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef SHARED_CONST_BUFFER_HPP__ 37 | #define SHARED_CONST_BUFFER_HPP__ 38 | 39 | // 40 | // reference_counted.hpp 41 | // ~~~~~~~~~~~~~~~~~~~~~ 42 | // 43 | // Copyright (c) 2003-2015 Christopher M. Kohlhoff (chris at kohlhoff dot com) 44 | // 45 | // Distributed under the Boost Software License, Version 1.0. (See accompanying 46 | // file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) 47 | // 48 | 49 | #include 50 | #include 51 | 52 | // A reference-counted non-modifiable buffer class. 53 | 54 | namespace hw_interface_support_types 55 | { 56 | class shared_const_buffer 57 | { 58 | public: 59 | // Construct from a std::string. 60 | shared_const_buffer(const std::string& data) 61 | : data_(new std::vector(data.begin(), data.end())), 62 | buffer_(new boost::asio::const_buffer(boost::asio::buffer(*data_))) 63 | { 64 | } 65 | 66 | shared_const_buffer(const char* data, const int length) 67 | : data_(new std::vector(data, data+length)), 68 | buffer_(new boost::asio::const_buffer(boost::asio::buffer(*data_))) 69 | { 70 | } 71 | 72 | shared_const_buffer(const char* data, const int length, bool LEtoBE) 73 | : data_(new std::vector(length)), 74 | buffer_(new boost::asio::const_buffer(boost::asio::buffer(*data_))) 75 | { 76 | if(LEtoBE) 77 | { 78 | for(int i = 0; i < length; i++) 79 | { 80 | data_->data()[i] = data[length-1-i]; 81 | } 82 | } 83 | } 84 | 85 | shared_const_buffer(const std::vector &data) 86 | : data_(new std::vector(data)), 87 | buffer_(new boost::asio::const_buffer(boost::asio::buffer(*data_))) 88 | { 89 | 90 | } 91 | 92 | template 93 | shared_const_buffer(const std::vector &data) 94 | : data_(new std::vector(data)), 95 | buffer_(new boost::asio::const_buffer(boost::asio::buffer(*data_))) 96 | { 97 | 98 | } 99 | 100 | template 101 | shared_const_buffer(const S &data) 102 | : data_(new std::vector(reinterpret_cast(&data), sizeof(S)+reinterpret_cast(&data))), 103 | buffer_(boost::asio::buffer(*data_)) 104 | { 105 | 106 | } 107 | 108 | 109 | // Implement the ConstBufferSequence requirements. 110 | typedef boost::asio::const_buffer value_type; 111 | typedef const boost::asio::const_buffer* const_iterator; 112 | const boost::asio::const_buffer* begin() const { return buffer_.get(); } 113 | const boost::asio::const_buffer* end() const { return buffer_.get() + 1; } 114 | 115 | protected: 116 | //protected empty constructor for implementation overlods if needed 117 | shared_const_buffer() {} 118 | std::shared_ptr > data_; 119 | std::shared_ptr buffer_; 120 | 121 | }; 122 | } 123 | 124 | #endif //SHARED_CONST_BUFFER_HPP__ 125 | 126 | --------------------------------------------------------------------------------