mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

renamed folder with '_v2' back, disabled ci for ros temporarily

Browse Source
This commit is contained in:
Ruixiang Du
2020-11-03 17:01:45 +08:00
parent 4d4b5baf73
commit 1c6783d539
18 changed files with 283 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
.gitlab-ci.yml
View File

@@ -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"
# 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"

2
ugv_sdk/CMakeLists.txt
View File

@@ -117,7 +117,7 @@ if(BUILD_WITHOUT_ROS)
endif()
# add app source directory
add_subdirectory(apps)
# add_subdirectory(apps)
endif()
# Build tests

6
ugv_sdk/apps/CMakeLists.txt
View File

@@ -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()
# if(BUILD_MONITOR)
# add_subdirectory(scout_monitor)
# endif()

9
ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp → ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp
View File

@@ -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);

2
ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h → ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/hunter_v2/hunter_protocol.h"
#include "ugv_sdk/hunter/hunter_protocol.h"
#ifdef __linux__
#include <linux/can.h>

0
ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h → ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h
View File

6
ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp → ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp
View File

@@ -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;

8
ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp → ugv_sdk/include/ugv_sdk/scout/scout_base.hpp
View File

@@ -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 {

2
ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h → ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/scout_v2/scout_protocol.h"
#include "ugv_sdk/scout/scout_protocol.h"
#ifdef __linux__
#include <linux/can.h>

0
ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h → ugv_sdk/include/ugv_sdk/scout/scout_protocol.h
View File

136
ugv_sdk/include/ugv_sdk/scout/scout_types.hpp Normal file
View File

@@ -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 <cstdint>
#include <iostream>
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 */

2
ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h → ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h
View File

@@ -18,7 +18,7 @@ extern "C" {
#include <string.h>
#include <stdbool.h>
#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);

140
ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp
View File

@@ -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 <cstdint>
#include <iostream>
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 */

119
ugv_sdk/src/hunter_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/hunter_v2/hunter_base.hpp"
#include "ugv_sdk/hunter/hunter_base.hpp"
#include <algorithm>
#include <array>
@@ -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<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity_height_byte = static_cast<int16_t>(linear_vel*1000)>>8;
current_motion_cmd_.linear_velocity_low_byte = static_cast<int16_t>(linear_vel*1000)&0xff;
current_motion_cmd_.angular_velocity_height_byte = static_cast<int16_t>(angular_vel*1000)>>8;
current_motion_cmd_.angular_velocity_low_byte = static_cast<int16_t>(angular_vel*1000)&0xff;
current_motion_cmd_.linear_velocity_height_byte =
static_cast<int16_t>(linear_vel * 1000) >> 8;
current_motion_cmd_.linear_velocity_low_byte =
static_cast<int16_t>(linear_vel * 1000) & 0xff;
current_motion_cmd_.angular_velocity_height_byte =
static_cast<int16_t>(angular_vel * 1000) >> 8;
current_motion_cmd_.angular_velocity_low_byte =
static_cast<int16_t>(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<uint16_t>(msg.data.status.current.low_byte) |
state.motor_hs_state[msg.motor_id].current =
(static_cast<uint16_t>(msg.data.status.current.low_byte) |
static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) /
10.0;
state.motor_H_state[msg.motor_id].rpm
= static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.rpm.low_byte) |
static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_H_state[msg.motor_id].motor_pose
= static_cast<int32_t>(
static_cast<uint32_t>(msg.data.status.moter_pose.lowest) |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest) << 8 |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)<< 16|
static_cast<uint32_t>(msg.data.status.moter_pose.heighest)<< 24 );
state.motor_hs_state[msg.motor_id].rpm = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.rpm.low_byte) |
static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_hs_state[msg.motor_id].motor_pose = static_cast<int32_t>(
static_cast<uint32_t>(msg.data.status.moter_pose.lowest) |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest) << 8 |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)
<< 16 |
static_cast<uint32_t>(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<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte) << 8) /
state.motor_ls_state[msg.motor_id].driver_voltage =
(static_cast<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte)
<< 8) /
10.0;
state.motor_L_state[msg.motor_id].driver_temperature
= static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(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<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(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:

2
ugv_sdk/src/hunter_can_parser.c
View File

@@ -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);

16
ugv_sdk/src/scout_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/scout_v2/scout_base.hpp"
#include "ugv_sdk/scout/scout_base.hpp"
#include <stdio.h>
#include <string>
#include <cstring>
@@ -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<uint16_t>(msg.data.status.current.low_byte) |
static_cast<uint16_t>(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<int16_t>(
static_cast<int16_t>(msg.data.status.rpm.low_byte) |
static_cast<int16_t>(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<int32_t>(static_cast<uint32_t>(msg.data.status.moter_pose.lowest)|static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest
)<<8|static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)<<16|static_cast<uint32_t>(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<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(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<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(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;

2
ugv_sdk/src/scout_can_parser.c
View File

@@ -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"

2
ugv_sdk/src/scout_uart_parser.c
View File

@@ -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