diff --git a/scout_msgs/CMakeLists.txt b/scout_msgs/CMakeLists.txt index 677001b..ac5330e 100644 --- a/scout_msgs/CMakeLists.txt +++ b/scout_msgs/CMakeLists.txt @@ -8,8 +8,9 @@ project(scout_msgs) ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS - std_msgs message_generation + message_runtime + std_msgs ) ## System dependencies are found with CMake's conventions @@ -47,19 +48,22 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - ScoutStatus.msg - ScoutMotorState.msg - ScoutLightState.msg + ScoutActuatorState.msg ScoutLightCmd.msg + ScoutMotionState.msg + ScoutRCState.msg + ScoutSystemState.msg ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + ScoutCheckLightState.srv + ScoutCheckRCState.srv + ScoutCheckSystemState.srv + ScoutCheckActuatorState.srv +) ## Generate actions in the 'action' folder # add_action_files( diff --git a/scout_msgs/msg/ScoutActuatorState.msg b/scout_msgs/msg/ScoutActuatorState.msg new file mode 100644 index 0000000..105391f --- /dev/null +++ b/scout_msgs/msg/ScoutActuatorState.msg @@ -0,0 +1,18 @@ +#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) +#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) +#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04) +#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08) +#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10) +#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20) +#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) +#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) + +uint8 motor_id +int16 rpm +float64 current +int32 pulse_count + +float32 driver_voltage +float32 driver_temperature +int8 motor_temperature +uint8 driver_state \ No newline at end of file diff --git a/scout_msgs/msg/ScoutLightCmd.msg b/scout_msgs/msg/ScoutLightCmd.msg index 4654271..ba16d86 100644 --- a/scout_msgs/msg/ScoutLightCmd.msg +++ b/scout_msgs/msg/ScoutLightCmd.msg @@ -1,9 +1,9 @@ -uint8 LIGHT_CONST_OFF = 0 -uint8 LIGHT_CONST_ON = 1 -uint8 LIGHT_BREATH = 2 -uint8 LIGHT_CUSTOM = 3 +# uint8 LIGHT_CONST_OFF = 0 +# uint8 LIGHT_CONST_ON = 1 +# uint8 LIGHT_BREATH = 2 +# uint8 LIGHT_CUSTOM = 3 -bool enable_cmd_light_control +bool cmd_ctrl_allowed uint8 front_mode uint8 front_custom_value uint8 rear_mode diff --git a/scout_msgs/msg/ScoutLightState.msg b/scout_msgs/msg/ScoutLightState.msg deleted file mode 100644 index cf67389..0000000 --- a/scout_msgs/msg/ScoutLightState.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 mode -uint8 custom_value \ No newline at end of file diff --git a/scout_msgs/msg/ScoutMotionState.msg b/scout_msgs/msg/ScoutMotionState.msg new file mode 100644 index 0000000..070f0a7 --- /dev/null +++ b/scout_msgs/msg/ScoutMotionState.msg @@ -0,0 +1,4 @@ +Header header + +float32 linear_velocity +float32 angular_velocity \ No newline at end of file diff --git a/scout_msgs/msg/ScoutMotorState.msg b/scout_msgs/msg/ScoutMotorState.msg deleted file mode 100644 index eca1c6d..0000000 --- a/scout_msgs/msg/ScoutMotorState.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 current -float64 rpm -float64 temperature \ No newline at end of file diff --git a/scout_msgs/msg/ScoutRCState.msg b/scout_msgs/msg/ScoutRCState.msg new file mode 100644 index 0000000..250f16d --- /dev/null +++ b/scout_msgs/msg/ScoutRCState.msg @@ -0,0 +1,10 @@ +uint8 swa +uint8 swb +uint8 swc +uint8 swd + +int8 stick_right_v +int8 stick_right_h +int8 stick_left_v +int8 stick_left_h +int8 var_a \ No newline at end of file diff --git a/scout_msgs/msg/ScoutStatus.msg b/scout_msgs/msg/ScoutStatus.msg deleted file mode 100644 index 3faa2ee..0000000 --- a/scout_msgs/msg/ScoutStatus.msg +++ /dev/null @@ -1,27 +0,0 @@ -Header header - -int8 MOTOR_ID_FRONT_RIGHT = 0 -int8 MOTOR_ID_FRONT_LEFT = 1 -int8 MOTOR_ID_REAR_RIGHT = 2 -int8 MOTOR_ID_REAR_LEFT = 3 - -int8 LIGHT_ID_FRONT = 0 -int8 LIGHT_ID_REAR = 1 - -# motion state -float64 linear_velocity -float64 angular_velocity - -# base state -uint8 base_state -uint8 control_mode -uint16 fault_code -float64 battery_voltage - -# motor state -ScoutMotorState[4] motor_states - -# light state -bool light_control_enabled -ScoutLightState front_light_state -ScoutLightState rear_light_state \ No newline at end of file diff --git a/scout_msgs/msg/ScoutSystemState.msg b/scout_msgs/msg/ScoutSystemState.msg new file mode 100644 index 0000000..2400aad --- /dev/null +++ b/scout_msgs/msg/ScoutSystemState.msg @@ -0,0 +1,23 @@ +# uint8 VehicleStateNormal = 0 +# uint8 VehicleStateEStop = 1 +# uint8 VehicleStateException = 2 + +# uint8 CONTROL_MODE_RC = 0 +# uint8 CONTROL_MODE_CAN = 1 +# uint8 CONTROL_MODE_UART = 2 + +# uint16 SYSTEM_ERROR_MOTOR_DRIVER_MASK 0x0100 +# uint16 SYSTEM_ERROR_HL_COMM_MASK 0x0200 +# uint16 SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) +# uint16 SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002) +# uint16 SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004) +# uint16 SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008) +# uint16 SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010) +# uint16 SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020) +# uint16 SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040) +# uint16 SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080) + +uint8 vehicle_state +uint8 control_mode +float32 battery_voltage +uint16 error_code \ No newline at end of file diff --git a/scout_msgs/package.xml b/scout_msgs/package.xml index da66136..5809d39 100644 --- a/scout_msgs/package.xml +++ b/scout_msgs/package.xml @@ -20,6 +20,9 @@ std_msgs message_runtime std_msgs + std_msgs + message_runtime + std_msgs diff --git a/scout_msgs/srv/ScoutCheckActuatorState.srv b/scout_msgs/srv/ScoutCheckActuatorState.srv new file mode 100644 index 0000000..74857e1 --- /dev/null +++ b/scout_msgs/srv/ScoutCheckActuatorState.srv @@ -0,0 +1,10 @@ +uint8 motor_id +--- +int16 rpm +float64 current +int32 pulse_count + +float32 driver_voltage +float32 driver_temperature +int8 motor_temperature +uint8 driver_state \ No newline at end of file diff --git a/scout_msgs/srv/ScoutCheckLightState.srv b/scout_msgs/srv/ScoutCheckLightState.srv new file mode 100644 index 0000000..46669cb --- /dev/null +++ b/scout_msgs/srv/ScoutCheckLightState.srv @@ -0,0 +1,12 @@ +bool is_check +--- +bool cmd_ctrl_allowed +uint8 front_mode +uint8 front_custom_value +uint8 rear_mode +uint8 rear_custom_value + +# uint8 LIGHT_CONST_OFF = 0 +# uint8 LIGHT_CONST_ON = 1 +# uint8 LIGHT_BREATH = 2 +# uint8 LIGHT_CUSTOM = 3 diff --git a/scout_msgs/srv/ScoutCheckRCState.srv b/scout_msgs/srv/ScoutCheckRCState.srv new file mode 100644 index 0000000..9cf433a --- /dev/null +++ b/scout_msgs/srv/ScoutCheckRCState.srv @@ -0,0 +1,12 @@ +bool is_check +--- +uint8 swa +uint8 swb +uint8 swc +uint8 swd + +int8 stick_right_v +int8 stick_right_h +int8 stick_left_v +int8 stick_left_h +int8 var_a \ No newline at end of file diff --git a/scout_msgs/srv/ScoutCheckSystemState.srv b/scout_msgs/srv/ScoutCheckSystemState.srv new file mode 100644 index 0000000..9d1a70a --- /dev/null +++ b/scout_msgs/srv/ScoutCheckSystemState.srv @@ -0,0 +1,25 @@ +bool is_check +--- +uint8 vehicle_state +uint8 control_mode +float32 battery_voltage +uint16 error_code + +# uint8 VehicleStateNormal = 0 +# uint8 VehicleStateEStop = 1 +# uint8 VehicleStateException = 2 + +# uint8 CONTROL_MODE_RC = 0 +# uint8 CONTROL_MODE_CAN = 1 +# uint8 CONTROL_MODE_UART = 2 + +# uint16 SYSTEM_ERROR_MOTOR_DRIVER_MASK 0x0100 +# uint16 SYSTEM_ERROR_HL_COMM_MASK 0x0200 +# uint16 SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) +# uint16 SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002) +# uint16 SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004) +# uint16 SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008) +# uint16 SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010) +# uint16 SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020) +# uint16 SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040) +# uint16 SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080) \ No newline at end of file