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