From 1c6783d539f790403852ab11b5cbbc2a47a3669b Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 3 Nov 2020 17:01:45 +0800 Subject: [PATCH] renamed folder with '_v2' back, disabled ci for ros temporarily --- .gitlab-ci.yml | 108 +++++++------- ugv_sdk/CMakeLists.txt | 2 +- ugv_sdk/apps/CMakeLists.txt | 6 +- .../{hunter_v2 => hunter}/hunter_base.hpp | 9 +- .../{hunter_v2 => hunter}/hunter_can_parser.h | 2 +- .../{hunter_v2 => hunter}/hunter_protocol.h | 0 .../{hunter_v2 => hunter}/hunter_types.hpp | 6 +- .../{scout_v2 => scout}/scout_base.hpp | 8 +- .../{scout_v2 => scout}/scout_can_parser.h | 2 +- .../{scout_v2 => scout}/scout_protocol.h | 0 ugv_sdk/include/ugv_sdk/scout/scout_types.hpp | 136 +++++++++++++++++ .../{scout_v2 => scout}/scout_uart_parser.h | 2 +- .../include/ugv_sdk/scout_v2/scout_types.hpp | 140 ------------------ ugv_sdk/src/hunter_base.cpp | 119 ++++++++------- ugv_sdk/src/hunter_can_parser.c | 2 +- ugv_sdk/src/scout_base.cpp | 16 +- ugv_sdk/src/scout_can_parser.c | 2 +- ugv_sdk/src/scout_uart_parser.c | 2 +- 18 files changed, 283 insertions(+), 279 deletions(-) rename ugv_sdk/include/ugv_sdk/{hunter_v2 => hunter}/hunter_base.hpp (85%) rename ugv_sdk/include/ugv_sdk/{hunter_v2 => hunter}/hunter_can_parser.h (94%) rename ugv_sdk/include/ugv_sdk/{hunter_v2 => hunter}/hunter_protocol.h (100%) rename ugv_sdk/include/ugv_sdk/{hunter_v2 => hunter}/hunter_types.hpp (94%) rename ugv_sdk/include/ugv_sdk/{scout_v2 => scout}/scout_base.hpp (90%) rename ugv_sdk/include/ugv_sdk/{scout_v2 => scout}/scout_can_parser.h (94%) rename ugv_sdk/include/ugv_sdk/{scout_v2 => scout}/scout_protocol.h (100%) create mode 100644 ugv_sdk/include/ugv_sdk/scout/scout_types.hpp rename ugv_sdk/include/ugv_sdk/{scout_v2 => scout}/scout_uart_parser.h (92%) delete mode 100644 ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 42c231c..f29298e 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -27,62 +27,62 @@ focal-cpp-build: - cmake --build . && cpack ## Build and test as catkin package -kinetic-catkin-build: - stage: build - image: rduweston/ubuntu-ci:kinetic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" +# kinetic-catkin-build: +# stage: build +# image: rduweston/ubuntu-ci:kinetic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" -kinetic-catkin-isolated-build: - stage: build - image: rduweston/ubuntu-ci:kinetic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" +# kinetic-catkin-isolated-build: +# stage: build +# image: rduweston/ubuntu-ci:kinetic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" -melodic-catkin-build: - stage: build - image: rduweston/ubuntu-ci:melodic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" +# melodic-catkin-build: +# stage: build +# image: rduweston/ubuntu-ci:melodic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" -melodic-catkin-isolated-build: - stage: build - image: rduweston/ubuntu-ci:melodic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" +# melodic-catkin-isolated-build: +# stage: build +# image: rduweston/ubuntu-ci:melodic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" -noetic-catkin-build: - stage: build - image: rduweston/ubuntu-ci:noetic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" +# noetic-catkin-build: +# stage: build +# image: rduweston/ubuntu-ci:noetic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make" -noetic-catkin-isolated-build: - stage: build - image: rduweston/ubuntu-ci:noetic - script: - - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" - - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk - - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive - - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git - - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" \ No newline at end of file +# noetic-catkin-isolated-build: +# stage: build +# image: rduweston/ubuntu-ci:noetic +# script: +# - mkdir -p /catkin_ws/src && cd /catkin_ws/src && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace" +# - cp -r /builds/$CI_PROJECT_PATH /catkin_ws/src/wrp_sdk +# - cd /catkin_ws/src/wrp_sdk && git submodule update --init --recursive +# - cd /catkin_ws/src && git clone https://gitlab-ci-token:${CI_JOB_TOKEN}@gitlab.com/westonrobot/ros/scout_base.git +# - cd /catkin_ws && /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make_isolated --install" \ No newline at end of file diff --git a/ugv_sdk/CMakeLists.txt b/ugv_sdk/CMakeLists.txt index 6c45733..838b798 100755 --- a/ugv_sdk/CMakeLists.txt +++ b/ugv_sdk/CMakeLists.txt @@ -117,7 +117,7 @@ if(BUILD_WITHOUT_ROS) endif() # add app source directory - add_subdirectory(apps) + # add_subdirectory(apps) endif() # Build tests diff --git a/ugv_sdk/apps/CMakeLists.txt b/ugv_sdk/apps/CMakeLists.txt index ed97768..9263416 100755 --- a/ugv_sdk/apps/CMakeLists.txt +++ b/ugv_sdk/apps/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(app_hunter_demo ugv_sdk) # add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) # target_link_libraries(app_tracer_demo ugv_sdk) -if(BUILD_MONITOR) - add_subdirectory(scout_monitor) -endif() \ No newline at end of file +# if(BUILD_MONITOR) +# add_subdirectory(scout_monitor) +# endif() \ No newline at end of file diff --git a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp b/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp similarity index 85% rename from ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp rename to ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp index 68d0faa..55b2628 100644 --- a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp +++ b/ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp @@ -17,9 +17,9 @@ #include "ugv_sdk/mobile_base.hpp" -#include "ugv_sdk/hunter_v2/hunter_protocol.h" -#include "ugv_sdk/hunter_v2/hunter_can_parser.h" -#include "ugv_sdk/hunter_v2/hunter_types.hpp" +#include "ugv_sdk/hunter/hunter_protocol.h" +#include "ugv_sdk/hunter/hunter_can_parser.h" +#include "ugv_sdk/hunter/hunter_types.hpp" namespace westonrobot { class HunterBase : public MobileBase { @@ -31,7 +31,8 @@ class HunterBase : public MobileBase { HunterState GetHunterState(); // motion control - void SetMotionCommand(double linear_vel, double angular_vel, double steering_angle, + void SetMotionCommand(double linear_vel, double angular_vel, + double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag = HunterMotionCmd::FaultClearFlag::NO_FAULT); diff --git a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h b/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h similarity index 94% rename from ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h rename to ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h index 2c77ddd..f887be3 100644 --- a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h +++ b/ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/hunter_v2/hunter_protocol.h" +#include "ugv_sdk/hunter/hunter_protocol.h" #ifdef __linux__ #include diff --git a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h b/ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h similarity index 100% rename from ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h rename to ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h diff --git a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp b/ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp similarity index 94% rename from ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp rename to ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp index 953ae8c..28d9350 100644 --- a/ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp +++ b/ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp @@ -17,7 +17,7 @@ namespace westonrobot { struct HunterState { - struct MotorHeightSpeedState + struct MotorHighSpeedState { double current = 0; // in A double rpm = 0; @@ -42,8 +42,8 @@ struct HunterState // motor state static constexpr uint8_t motor_num = 3; - MotorHeightSpeedState motor_H_state[motor_num]; - MotorLowSpeedState motor_L_state[motor_num]; + MotorHighSpeedState motor_hs_state[motor_num]; + MotorLowSpeedState motor_ls_state[motor_num]; // motion state double linear_velocity = 0; diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp similarity index 90% rename from ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp rename to ugv_sdk/include/ugv_sdk/scout/scout_base.hpp index 7f914ea..ea82e43 100644 --- a/ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp +++ b/ugv_sdk/include/ugv_sdk/scout/scout_base.hpp @@ -18,10 +18,10 @@ #include "ugv_sdk/mobile_base.hpp" -#include "ugv_sdk/scout_v2/scout_protocol.h" -#include "ugv_sdk/scout_v2/scout_can_parser.h" -#include "ugv_sdk/scout_v2/scout_uart_parser.h" -#include "ugv_sdk/scout_v2/scout_types.hpp" +#include "ugv_sdk/scout/scout_protocol.h" +#include "ugv_sdk/scout/scout_can_parser.h" +#include "ugv_sdk/scout/scout_uart_parser.h" +#include "ugv_sdk/scout/scout_types.hpp" namespace westonrobot { class ScoutBase : public MobileBase { diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h b/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h similarity index 94% rename from ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h rename to ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h index 17f6244..0e44f25 100644 --- a/ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h +++ b/ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h @@ -17,7 +17,7 @@ extern "C" { #include #include -#include "ugv_sdk/scout_v2/scout_protocol.h" +#include "ugv_sdk/scout/scout_protocol.h" #ifdef __linux__ #include diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h b/ugv_sdk/include/ugv_sdk/scout/scout_protocol.h similarity index 100% rename from ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h rename to ugv_sdk/include/ugv_sdk/scout/scout_protocol.h diff --git a/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp new file mode 100644 index 0000000..04bc481 --- /dev/null +++ b/ugv_sdk/include/ugv_sdk/scout/scout_types.hpp @@ -0,0 +1,136 @@ +/* + * scout_state.hpp + * + * Created on: Jun 11, 2019 08:48 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_STATE_HPP +#define SCOUT_STATE_HPP + +#include +#include + +namespace westonrobot { +struct ScoutState { + enum MotorID { + FRONT_RIGHT = 0, + FRONT_LEFT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 + }; + + struct MotorState { + double current = 0; // in A + double rpm = 0; + double temperature = 0; + double motor_pose = 0; + }; + struct DriverState { + double driver_voltage = 0; + double driver_temperature = 0; + uint8_t driver_state = 0; + }; + struct MotorHighSpeedState { + double current = 0; // in A + double rpm = 0; + double motor_pose = 0; + }; + struct MotorLowSpeedState { + double driver_voltage = 0; + double driver_temperature = 0; + double motor_temperature = 0; + uint8_t driver_state = 0; + }; + struct LightState { + uint8_t mode = 0; + uint8_t custom_value = 0; + }; + + // base state + uint8_t base_state = 0; + uint8_t control_mode = 0; + uint8_t fault_code = 0; + double battery_voltage = 0.0; + + // motor state + static constexpr uint8_t motor_num = 4; + // MotorState motor_states[motor_num]; + // DriverState driver_states[motor_num]; + MotorHighSpeedState motor_hs_state[motor_num]; + MotorLowSpeedState motor_ls_state[motor_num]; + + // light state + bool light_control_enabled = false; + LightState front_light_state; + LightState rear_light_state; + + // motion state + double linear_velocity = 0; + double angular_velocity = 0; + + // odometer state + double left_odomter = 0; + double right_odomter = 0; +}; + +struct ScoutMotionCmd { + enum class FaultClearFlag { + NO_FAULT = 0x00, + BAT_UNDER_VOL = 0x01, + BAT_OVER_VOL = 0x02, + MOTOR1_COMM = 0x03, + MOTOR2_COMM = 0x04, + MOTOR3_COMM = 0x05, + MOTOR4_COMM = 0x06, + MOTOR_DRV_OVERHEAT = 0x07, + MOTOR_OVERCURRENT = 0x08 + }; + + ScoutMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0, + int8_t angular_height_byte = 0, int8_t angular_low_byte = 0, + FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) + : linear_velocity_height_byte(linear_height_byte), + linear_velocity_low_byte(linear_low_byte), + angular_velocity_height_byte(angular_height_byte), + angular_velocity_low_byte(angular_low_byte), + fault_clear_flag(fault_clr_flag) {} + + int8_t linear_velocity_height_byte; + int8_t linear_velocity_low_byte; + int8_t angular_velocity_height_byte; + int8_t angular_velocity_low_byte; + FaultClearFlag fault_clear_flag; + + static constexpr double max_linear_velocity = 1.5; // 1.5 m/s + static constexpr double min_linear_velocity = -1.5; // -1.5 m/s + static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s + static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s +}; + +struct ScoutLightCmd { + enum class LightMode { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 + }; + + ScoutLightCmd() = default; + ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, + uint8_t r_value) + : front_mode(f_mode), + front_custom_value(f_value), + rear_mode(r_mode), + rear_custom_value(r_value) {} + + LightMode front_mode; + uint8_t front_custom_value; + LightMode rear_mode; + uint8_t rear_custom_value; +}; +} // namespace westonrobot + +#endif /* SCOUT_STATE_HPP */ diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h b/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h similarity index 92% rename from ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h rename to ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h index 1524087..26fb95b 100644 --- a/ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h +++ b/ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h @@ -18,7 +18,7 @@ extern "C" { #include #include -#include "ugv_sdk/scout_v2/scout_protocol.h" +#include "ugv_sdk/scout/scout_protocol.h" bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg); void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len); diff --git a/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp b/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp deleted file mode 100644 index f39d030..0000000 --- a/ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/* - * scout_state.hpp - * - * Created on: Jun 11, 2019 08:48 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_STATE_HPP -#define SCOUT_STATE_HPP - -#include -#include - -namespace westonrobot -{ -struct ScoutState -{ - enum MotorID - { - FRONT_RIGHT = 0, - FRONT_LEFT = 1, - REAR_LEFT = 2, - REAR_RIGHT = 3 - }; - - struct MotorState - { - double current = 0; // in A - double rpm = 0; - double temperature = 0; - double motor_pose=0; - }; - struct DriverState - { - double driver_voltage = 0; - double driver_temperature = 0; - uint8_t driver_state = 0; - }; - struct MotorHeightSpeedState - { - double current = 0; // in A - double rpm = 0; - double motor_pose = 0; - }; - struct MotorLowSpeedState - { - double driver_voltage = 0; - double driver_temperature = 0; - double motor_temperature = 0; - uint8_t driver_state = 0; - }; - struct LightState - { - uint8_t mode = 0; - uint8_t custom_value = 0; - }; - - // base state - uint8_t base_state = 0; - uint8_t control_mode = 0; - uint8_t fault_code = 0; - double battery_voltage = 0.0; - - // motor state - static constexpr uint8_t motor_num = 4; -// MotorState motor_states[motor_num]; -// DriverState driver_states[motor_num]; - MotorHeightSpeedState motor_H_state[motor_num]; - MotorLowSpeedState motor_L_state[motor_num]; - - // light state - bool light_control_enabled = false; - LightState front_light_state; - LightState rear_light_state; - - // motion state - double linear_velocity = 0; - double angular_velocity = 0; - - //odometer state - double left_odomter=0; - double right_odomter=0; -}; - -struct ScoutMotionCmd -{ - enum class FaultClearFlag - { - NO_FAULT = 0x00, - BAT_UNDER_VOL = 0x01, - BAT_OVER_VOL = 0x02, - MOTOR1_COMM = 0x03, - MOTOR2_COMM = 0x04, - MOTOR3_COMM = 0x05, - MOTOR4_COMM = 0x06, - MOTOR_DRV_OVERHEAT = 0x07, - MOTOR_OVERCURRENT = 0x08 - }; - - ScoutMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0,int8_t angular_height_byte = 0,int8_t angular_low_byte = 0, - FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) - : linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte), - fault_clear_flag(fault_clr_flag) {} - - int8_t linear_velocity_height_byte; - int8_t linear_velocity_low_byte; - int8_t angular_velocity_height_byte; - int8_t angular_velocity_low_byte; - FaultClearFlag fault_clear_flag; - - static constexpr double max_linear_velocity = 1.5; // 1.5 m/s - static constexpr double min_linear_velocity = -1.5; // -1.5 m/s - static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s - static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s -}; - -struct ScoutLightCmd -{ - enum class LightMode - { - CONST_OFF = 0x00, - CONST_ON = 0x01, - BREATH = 0x02, - CUSTOM = 0x03 - }; - - ScoutLightCmd() = default; - ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), - rear_mode(r_mode), rear_custom_value(r_value) {} - - LightMode front_mode; - uint8_t front_custom_value; - LightMode rear_mode; - uint8_t rear_custom_value; -}; -} // namespace westonrobot - -#endif /* SCOUT_STATE_HPP */ diff --git a/ugv_sdk/src/hunter_base.cpp b/ugv_sdk/src/hunter_base.cpp index eee90fe..5fc1cf0 100644 --- a/ugv_sdk/src/hunter_base.cpp +++ b/ugv_sdk/src/hunter_base.cpp @@ -1,4 +1,4 @@ -#include "ugv_sdk/hunter_v2/hunter_base.hpp" +#include "ugv_sdk/hunter/hunter_base.hpp" #include #include @@ -28,7 +28,8 @@ void HunterBase::SendMotionCmd(uint8_t count) { /*if (can_connected_) m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; else if (serial_connected_) - m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/; + m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/ + ; motion_cmd_mutex_.lock(); m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.high_byte = current_motion_cmd_.linear_velocity_height_byte; @@ -45,7 +46,6 @@ void HunterBase::SendMotionCmd(uint8_t count) { m_msg.body.motion_control_msg.data.cmd.reserved2 = 0; m_msg.body.motion_control_msg.data.cmd.reserved3 = 0; - // send to can bus if (can_connected_) { can_frame m_frame; @@ -60,7 +60,7 @@ HunterState HunterBase::GetHunterState() { } void HunterBase::SetMotionCommand( - double linear_vel, double angular_vel,double steering_angle, + double linear_vel, double angular_vel, double steering_angle, HunterMotionCmd::FaultClearFlag fault_clr_flag) { // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); @@ -75,64 +75,69 @@ void HunterBase::SetMotionCommand( steering_angle = HunterMotionCmd::max_steering_angle; std::lock_guard guard(motion_cmd_mutex_); - current_motion_cmd_.linear_velocity_height_byte = static_cast(linear_vel*1000)>>8; - current_motion_cmd_.linear_velocity_low_byte = static_cast(linear_vel*1000)&0xff; - current_motion_cmd_.angular_velocity_height_byte = static_cast(angular_vel*1000)>>8; - current_motion_cmd_.angular_velocity_low_byte = static_cast(angular_vel*1000)&0xff; + current_motion_cmd_.linear_velocity_height_byte = + static_cast(linear_vel * 1000) >> 8; + current_motion_cmd_.linear_velocity_low_byte = + static_cast(linear_vel * 1000) & 0xff; + current_motion_cmd_.angular_velocity_height_byte = + static_cast(angular_vel * 1000) >> 8; + current_motion_cmd_.angular_velocity_low_byte = + static_cast(angular_vel * 1000) & 0xff; current_motion_cmd_.fault_clear_flag = fault_clr_flag; FeedCmdTimeoutWatchdog(); } -void HunterBase::SendModeCtl(){ +void HunterBase::SendModeCtl() { HunterMessage m_msg; m_msg.type = HunterControlModeMsg; mode_cmd_mutex_.lock(); - m_msg.body.mode_cmd_msg.data.cmd.mode_control=0x01; + m_msg.body.mode_cmd_msg.data.cmd.mode_control = 0x01; mode_cmd_mutex_.unlock(); - m_msg.body.mode_cmd_msg.data.cmd.reserved0=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved1=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved2=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved3=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved4=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved5=0; - m_msg.body.mode_cmd_msg.data.cmd.reserved6=0; + m_msg.body.mode_cmd_msg.data.cmd.reserved0 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved1 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved2 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved3 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved4 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved5 = 0; + m_msg.body.mode_cmd_msg.data.cmd.reserved6 = 0; if (can_connected_) { // send to can bus can_frame m_frame; EncodeHunterMsgToCAN(&m_msg, &m_frame); can_if_->SendFrame(m_frame); - }else {} + } else { + } } -void HunterBase::SetParkMode(){ +void HunterBase::SetParkMode() { HunterMessage m_msg; m_msg.type = HunterParkControlMsg; bool flag = current_motion_cmd_.linear_velocity_height_byte || current_motion_cmd_.linear_velocity_low_byte; - if(flag) - { + if (flag) { pack_mode_cmd_mutex_.lock(); - m_msg.body.park_control_msg.data.cmd.packing_mode=0x00; + m_msg.body.park_control_msg.data.cmd.packing_mode = 0x00; pack_mode_cmd_mutex_.unlock(); - }else { + } else { pack_mode_cmd_mutex_.lock(); - m_msg.body.park_control_msg.data.cmd.packing_mode=0x01; + m_msg.body.park_control_msg.data.cmd.packing_mode = 0x01; pack_mode_cmd_mutex_.unlock(); } - m_msg.body.park_control_msg.data.cmd.reserved0=0; - m_msg.body.park_control_msg.data.cmd.reserved1=0; - m_msg.body.park_control_msg.data.cmd.reserved2=0; - m_msg.body.park_control_msg.data.cmd.reserved3=0; - m_msg.body.park_control_msg.data.cmd.reserved4=0; - m_msg.body.park_control_msg.data.cmd.reserved5=0; - m_msg.body.park_control_msg.data.cmd.reserved6=0; + m_msg.body.park_control_msg.data.cmd.reserved0 = 0; + m_msg.body.park_control_msg.data.cmd.reserved1 = 0; + m_msg.body.park_control_msg.data.cmd.reserved2 = 0; + m_msg.body.park_control_msg.data.cmd.reserved3 = 0; + m_msg.body.park_control_msg.data.cmd.reserved4 = 0; + m_msg.body.park_control_msg.data.cmd.reserved5 = 0; + m_msg.body.park_control_msg.data.cmd.reserved6 = 0; if (can_connected_) { // send to can bus can_frame m_frame; EncodeHunterMsgToCAN(&m_msg, &m_frame); can_if_->SendFrame(m_frame); - }else {} + } else { + } } void HunterBase::ParseCANFrame(can_frame *rx_frame) { @@ -185,24 +190,24 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, break; } case HunterMotorDriverHeightSpeedStatusMsg: { - // std::cout << "motor driver height speed feedback received" << std::endl; + // std::cout << "motor driver height speed feedback received" << + // std::endl; const MotorDriverHeightSpeedStatusMessage &msg = status_msg.body.motor_driver_height_speed_status_msg; for (int i = 0; i < HunterState::motor_num; ++i) { - state.motor_H_state[msg.motor_id].current - =(static_cast(msg.data.status.current.low_byte) | + state.motor_hs_state[msg.motor_id].current = + (static_cast(msg.data.status.current.low_byte) | static_cast(msg.data.status.current.high_byte) << 8) / 10.0; - state.motor_H_state[msg.motor_id].rpm - = static_cast( - static_cast(msg.data.status.rpm.low_byte) | - static_cast(msg.data.status.rpm.high_byte) << 8); - state.motor_H_state[msg.motor_id].motor_pose - = static_cast( - static_cast(msg.data.status.moter_pose.lowest) | - static_cast(msg.data.status.moter_pose.sec_lowest) << 8 | - static_cast(msg.data.status.moter_pose.sec_heighest)<< 16| - static_cast(msg.data.status.moter_pose.heighest)<< 24 ); + state.motor_hs_state[msg.motor_id].rpm = static_cast( + static_cast(msg.data.status.rpm.low_byte) | + static_cast(msg.data.status.rpm.high_byte) << 8); + state.motor_hs_state[msg.motor_id].motor_pose = static_cast( + static_cast(msg.data.status.moter_pose.lowest) | + static_cast(msg.data.status.moter_pose.sec_lowest) << 8 | + static_cast(msg.data.status.moter_pose.sec_heighest) + << 16 | + static_cast(msg.data.status.moter_pose.heighest) << 24); } break; } @@ -211,18 +216,20 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg, const MotorDriverLowSpeedStatusMessage &msg = status_msg.body.motor_driver_low_speed_status_msg; for (int i = 0; i < HunterState::motor_num; ++i) { - state.motor_L_state[msg.motor_id].driver_voltage - =(static_cast(msg.data.status.driver_voltage.low_byte) | - static_cast(msg.data.status.driver_voltage.high_byte) << 8) / + state.motor_ls_state[msg.motor_id].driver_voltage = + (static_cast(msg.data.status.driver_voltage.low_byte) | + static_cast(msg.data.status.driver_voltage.high_byte) + << 8) / 10.0; - state.motor_L_state[msg.motor_id].driver_temperature - = static_cast( - static_cast(msg.data.status.driver_temperature.low_byte) | - static_cast(msg.data.status.driver_temperature.high_byte) << 8); - state.motor_L_state[msg.motor_id].motor_temperature - = msg.data.status.motor_temperature; - state.motor_L_state[msg.motor_id].driver_state - = msg.data.status.driver_status; + state.motor_ls_state[msg.motor_id] + .driver_temperature = static_cast( + static_cast(msg.data.status.driver_temperature.low_byte) | + static_cast(msg.data.status.driver_temperature.high_byte) + << 8); + state.motor_ls_state[msg.motor_id].motor_temperature = + msg.data.status.motor_temperature; + state.motor_ls_state[msg.motor_id].driver_state = + msg.data.status.driver_status; } } default: diff --git a/ugv_sdk/src/hunter_can_parser.c b/ugv_sdk/src/hunter_can_parser.c index cc77f9f..d7ffb55 100644 --- a/ugv_sdk/src/hunter_can_parser.c +++ b/ugv_sdk/src/hunter_can_parser.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/hunter_v2/hunter_can_parser.h" +#include "ugv_sdk/hunter/hunter_can_parser.h" #include "string.h" static void EncodeHunterMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); diff --git a/ugv_sdk/src/scout_base.cpp b/ugv_sdk/src/scout_base.cpp index 05b70ca..77ba039 100644 --- a/ugv_sdk/src/scout_base.cpp +++ b/ugv_sdk/src/scout_base.cpp @@ -1,4 +1,4 @@ -#include "ugv_sdk/scout_v2/scout_base.hpp" +#include "ugv_sdk/scout/scout_base.hpp" #include #include #include @@ -294,16 +294,16 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, // std::cout << "motor 1 driver feedback received" << std::endl; const MotorDriverHeightSpeedStatusMessage &msg =status_msg.body.motor_driver_height_speed_status_msg; for (int i = 0; i < ScoutState::motor_num; ++i) { - state.motor_H_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] + state.motor_hs_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] .current = (static_cast(msg.data.status.current.low_byte) | static_cast(msg.data.status.current.high_byte) << 8) / 10.0; - state.motor_H_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] + state.motor_hs_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] .rpm = static_cast( static_cast(msg.data.status.rpm.low_byte) | static_cast(msg.data.status.rpm.high_byte) << 8); - state.motor_H_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] + state.motor_hs_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id] .motor_pose = static_cast(static_cast(msg.data.status.moter_pose.lowest)|static_cast(msg.data.status.moter_pose.sec_lowest )<<8|static_cast(msg.data.status.moter_pose.sec_heighest)<<16|static_cast(msg.data.status.moter_pose.heighest)<<24); } @@ -313,18 +313,18 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, // std::cout << "motor 1 driver feedback received" << std::endl; const MotorDriverLowSpeedStatusMessage &msg =status_msg.body.motor_driver_low_speed_status_msg; for (int i = 0; i < ScoutState::motor_num; ++i) { - state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + state.motor_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] .driver_voltage = (static_cast(msg.data.status.driver_voltage.low_byte) | static_cast(msg.data.status.driver_voltage.high_byte) << 8) / 10.0; - state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + state.motor_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] .driver_temperature = static_cast( static_cast(msg.data.status.driver_temperature.low_byte) | static_cast(msg.data.status.driver_temperature.high_byte) << 8); - state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + state.motor_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] .motor_temperature = msg.data.status.motor_temperature; - state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] + state.motor_ls_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id] .driver_state = msg.data.status.driver_state; } break; diff --git a/ugv_sdk/src/scout_can_parser.c b/ugv_sdk/src/scout_can_parser.c index 06bab5f..bb4a5a3 100644 --- a/ugv_sdk/src/scout_can_parser.c +++ b/ugv_sdk/src/scout_can_parser.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/scout_v2/scout_can_parser.h" +#include "ugv_sdk/scout/scout_can_parser.h" #include "string.h" diff --git a/ugv_sdk/src/scout_uart_parser.c b/ugv_sdk/src/scout_uart_parser.c index cb015b7..b8184e5 100644 --- a/ugv_sdk/src/scout_uart_parser.c +++ b/ugv_sdk/src/scout_uart_parser.c @@ -7,7 +7,7 @@ * Copyright (c) 2019 Ruixiang Du (rdu) */ -#include "ugv_sdk/scout_v2/scout_uart_parser.h" +#include "ugv_sdk/scout/scout_uart_parser.h" // #define USE_XOR_CHECKSUM