mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 03:15:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

tested work with both scout_base_node and scout_messenger

Browse Source
This commit is contained in:
pinloon.lee
2021-04-01 15:48:53 +08:00
parent 774b2731b5
commit 840739d4fe
4 changed files with 105 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
scout_base/CMakeLists.txt
View File

@@ -24,7 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
INCLUDE_DIRS include
# LIBRARIES scout_messenger
LIBRARIES scout_messenger
CATKIN_DEPENDS ugv_sdk scout_msgs
# DEPENDS Boost
)
@@ -40,13 +40,12 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
# add_library(scout_messenger STATIC src/scout_messenger.cpp)
# target_link_libraries(scout_messenger ${catkin_LIBRARIES})
# add_dependencies(scout_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(scout_messenger STATIC src/scout_messenger.cpp)
target_link_libraries(scout_messenger ${catkin_LIBRARIES})
add_dependencies(scout_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(scout_base_node src/scout_base_node.cpp)
target_link_libraries(scout_base_node ${catkin_LIBRARIES})
add_dependencies(scout_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
#############
## Install ##

2
scout_base/include/scout_base/scout_messenger.hpp
View File

@@ -18,7 +18,7 @@
#include <tf2_ros/transform_broadcaster.h>
#include "scout_msgs/ScoutLightCmd.h"
#include "ugv_sdk/scout/scout_base.hpp"
#include "ugv_sdk/scout_base.hpp"
namespace westonrobot
{

195
scout_base/src/scout_base_node.cpp
View File

@@ -5,175 +5,58 @@
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include "scout_msgs/ScoutMotionState.h"
#include "scout_msgs/ScoutLightCmd.h"
#include "scout_msgs/ScoutCheckActuatorState.h"
#include "scout_msgs/ScoutCheckLightState.h"
#include "scout_msgs/ScoutCheckRCState.h"
#include "scout_msgs/ScoutCheckSystemState.h"
#include "ugv_sdk/scout_base.hpp"
#include "scout_base/scout_messenger.hpp"
using namespace westonrobot;
typedef enum {
SYSTEM_MSG = 0,
MOTION_MSG,
LIGHT_MSG,
RC_MSG,
ACTUATOR_MSG
} MsgsType;
std::shared_ptr<ScoutBase> robot;
void ControlCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
robot->SetMotionCommand(msg->linear.x, msg->angular.z);
ROS_INFO("received motion control cmd");
}
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
ScoutLightCmd cmd;
cmd.cmd_ctrl_allowed = msg->cmd_ctrl_allowed;
cmd.front_mode = (LightMode)msg->front_mode;
cmd.front_custom_value = msg->front_custom_value;
cmd.rear_mode = (LightMode)msg->rear_mode;
cmd.rear_custom_value = msg->rear_custom_value;
robot->SetLightCommand(cmd);
ROS_INFO("received light control cmd");
}
void PublishMsgTOROS(const ros::Publisher *pub, MsgsType msg_type) {
ScoutState scout_state = robot->GetScoutState();
switch (msg_type) {
case SYSTEM_MSG: {
break;
}
case MOTION_MSG: {
scout_msgs::ScoutMotionState msg;
msg.header.stamp = ros::Time::now();
msg.linear_velocity = scout_state.motion_state.linear_velocity;
msg.angular_velocity = scout_state.motion_state.angular_velocity;
pub->publish(msg);
break;
}
case LIGHT_MSG: {
break;
}
case RC_MSG: {
break;
}
case ACTUATOR_MSG: {
break;
}
default:
break;
}
}
bool CheckActuatorState(scout_msgs::ScoutCheckActuatorState::Request &req,
scout_msgs::ScoutCheckActuatorState::Response &res) {
ScoutState scout_state = robot->GetScoutState();
uint8_t motor_id = req.motor_id;
res.rpm = scout_state.actuator_hs_state[motor_id].rpm;
res.current = scout_state.actuator_hs_state[motor_id].current;
res.pulse_count = scout_state.actuator_hs_state[motor_id].pulse_count;
res.driver_voltage = scout_state.actuator_ls_state[motor_id].driver_voltage;
res.driver_temperature =
scout_state.actuator_ls_state[motor_id].driver_temperature;
res.motor_temperature =
scout_state.actuator_ls_state[motor_id].motor_temperature;
res.driver_state == scout_state.actuator_ls_state[motor_id].driver_state;
ROS_INFO("Actuator %d state check request",motor_id);
return true;
}
bool CheckLightState(scout_msgs::ScoutCheckLightState::Request &req,
scout_msgs::ScoutCheckLightState::Response &res) {
ScoutState scout_state = robot->GetScoutState();
res.cmd_ctrl_allowed = scout_state.light_state.cmd_ctrl_allowed;
res.front_mode = scout_state.light_state.front_light.mode;
res.front_custom_value = scout_state.light_state.front_light.custom_value;
res.rear_mode = scout_state.light_state.rear_light.mode;
res.rear_custom_value = scout_state.light_state.rear_light.custom_value;
ROS_INFO("Light state check request");
return true;
}
bool CheckRCState(scout_msgs::ScoutCheckRCState::Request &req,
scout_msgs::ScoutCheckRCState::Response &res) {
ScoutState scout_state = robot->GetScoutState();
res.swa = scout_state.rc_state.swa;
res.swb = scout_state.rc_state.swb;
res.swc = scout_state.rc_state.swc;
res.swd = scout_state.rc_state.swd;
res.stick_right_v = scout_state.rc_state.stick_right_v;
res.stick_right_h = scout_state.rc_state.stick_right_h;
res.stick_left_v = scout_state.rc_state.stick_left_v;
res.stick_left_h = scout_state.rc_state.stick_left_h;
res.var_a = scout_state.rc_state.var_a;
ROS_INFO("Light state check request");
return true;
}
bool CheckSystemState(scout_msgs::ScoutCheckSystemState::Request &req,
scout_msgs::ScoutCheckSystemState::Response &res) {
ScoutState scout_state = robot->GetScoutState();
res.vehicle_state = scout_state.system_state.vehicle_state;
res.control_mode = scout_state.system_state.control_mode;
res.battery_voltage = scout_state.system_state.battery_voltage;
res.error_code = scout_state.system_state.error_code;
ROS_INFO("Light state check request");
return true;
}
int main(int argc, char **argv) {
// setup ROS node
ros::init(argc, argv, "scout_node");
// ros::NodeHandle node;
ros::NodeHandle node(""), private_node("~");
// connect robot device
std::string device_name;
private_node.param<std::string>("port_name", device_name, std::string("can0"));
// instantiate a robot object
robot = std::make_shared<ScoutBase>();
robot->Connect(device_name);
robot->EnableCommandedMode();
ScoutROSMessenger messenger(robot.get(), &node);
// publisher
ros::Publisher state_pub = node.advertise<scout_msgs::ScoutMotionState>(
"/scout_motion_state", 10);
// fetch parameters before connecting to robot
std::string port_name;
private_node.param<std::string>("port_name", port_name, std::string("can0"));
private_node.param<std::string>("odom_frame", messenger.odom_frame_,
std::string("odom"));
private_node.param<std::string>("base_frame", messenger.base_frame_,
std::string("base_link"));
private_node.param<bool>("simulated_robot", messenger.simulated_robot_,
false);
private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
private_node.param<std::string>("odom_topic_name", messenger.odom_topic_name_,
std::string("odom"));
// subscriber
ros::Subscriber motion_cmd_sub =
node.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, ControlCmdCallback);
ros::Subscriber light_cmd_sub = node.subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, LightCmdCallback);
// servicer
ros::ServiceServer service1 =
node.advertiseService("check_scout_actuator", CheckActuatorState);
ros::ServiceServer service2 =
node.advertiseService("check_scout_light", CheckLightState);
ros::ServiceServer service3 =
node.advertiseService("check_scout_RC", CheckRCState);
ros::ServiceServer service4 =
node.advertiseService("check_scout_system", CheckSystemState);
ros::Rate loop_rate(50);
while (ros::ok()) {
PublishMsgTOROS(&state_pub, MOTION_MSG);
ros::spinOnce();
loop_rate.sleep();
if (!messenger.simulated_robot_) {
// connect to robot and setup ROS subscription
if (port_name.find("can") != std::string::npos) {
robot->Connect(port_name);
robot->EnableCommandedMode();
ROS_INFO("Using CAN bus to talk with the robot");
}
}
messenger.SetupSubscription();
// publish robot state at 50Hz while listening to twist commands
ros::Rate rate(50);
while (true) {
if (!messenger.simulated_robot_) {
messenger.PublishStateToROS();
} else {
double linear, angular;
messenger.GetCurrentMotionCmdForSim(linear, angular);
messenger.PublishSimStateToROS(linear, angular);
}
ros::spinOnce();
rate.sleep();
}
return 0;
}

109
scout_base/src/scout_messenger.cpp
View File

@@ -63,7 +63,7 @@ namespace westonrobot
{
if (!simulated_robot_)
{
if (msg->enable_cmd_light_control)
if (msg->cmd_ctrl_allowed)
{
ScoutLightCmd cmd;
@@ -71,57 +71,57 @@ namespace westonrobot
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
cmd.front_mode = CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
cmd.front_mode = CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
cmd.front_mode = BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.front_mode = CUSTOM;
cmd.front_custom_value = msg->front_custom_value;
break;
}
}
switch (msg->rear_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.rear_custom_value = msg->rear_custom_value;
break;
}
}
// not meant to be controlled by user for now
// switch (msg->rear_mode)
// {
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
// {
// cmd.rear_mode = CONST_OFF;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
// {
// cmd.rear_mode = CONST_ON;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
// {
// cmd.rear_mode = BREATH;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
// {
// cmd.rear_mode = CUSTOM;
// cmd.rear_custom_value = msg->rear_custom_value;
// break;
// }
// }
cmd.cmd_ctrl_allowed = true;
scout_->SetLightCommand(cmd);
}
else
{
scout_->DisableLightCmdControl();
scout_->DisableLightControl();
}
}
else
@@ -150,32 +150,43 @@ namespace westonrobot
status_msg.header.stamp = current_time_;
status_msg.linear_velocity = state.linear_velocity;
status_msg.angular_velocity = state.angular_velocity;
status_msg.linear_velocity = state.motion_state.linear_velocity;
status_msg.angular_velocity = state.motion_state.angular_velocity;
status_msg.base_state = state.base_state;
status_msg.control_mode = state.control_mode;
status_msg.fault_code = state.fault_code;
status_msg.battery_voltage = state.battery_voltage;
status_msg.vehicle_state = state.system_state.vehicle_state;
status_msg.control_mode = state.system_state.control_mode;
status_msg.error_code = state.system_state.error_code;
status_msg.battery_voltage = state.system_state.battery_voltage;
for (int i = 0; i < 4; ++i)
{
status_msg.motor_states[i].current = state.motor_states[i].current;
status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
// actuator_hs_state
uint8_t motor_id = state.actuator_hs_state[i].motor_id;
status_msg.actuator_states[motor_id].rpm = state.actuator_hs_state[i].rpm;
status_msg.actuator_states[motor_id].current = state.actuator_hs_state[i].current;
status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count;
// actuator_ls_state
motor_id = state.actuator_ls_state[i].motor_id;
status_msg.actuator_states[motor_id].driver_voltage = state.actuator_ls_state[i].driver_voltage;
status_msg.actuator_states[motor_id].driver_temperature = state.actuator_ls_state[i].driver_temperature;
status_msg.actuator_states[motor_id].motor_temperature = state.actuator_ls_state[i].motor_temperature;
status_msg.actuator_states[motor_id].driver_state = state.actuator_ls_state[i].driver_state;
}
status_msg.light_control_enabled = state.light_control_enabled;
status_msg.front_light_state.mode = state.front_light_state.mode;
status_msg.light_control_enabled = state.light_state.cmd_ctrl_allowed;
status_msg.front_light_state.mode = state.light_state.front_light.mode;
status_msg.front_light_state.custom_value =
state.front_light_state.custom_value;
status_msg.rear_light_state.mode = state.rear_light_state.mode;
state.light_state.front_light.custom_value;
status_msg.rear_light_state.mode = state.light_state.rear_light.mode;
status_msg.rear_light_state.custom_value =
state.front_light_state.custom_value;
state.light_state.rear_light.custom_value;
status_publisher_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
PublishOdometryToROS(state.motion_state.linear_velocity, state.motion_state.angular_velocity, dt);
// record time for next integration
last_time_ = current_time_;
@@ -203,9 +214,9 @@ namespace westonrobot
status_msg.linear_velocity = linear;
status_msg.angular_velocity = angular;
status_msg.base_state = 0x00;
status_msg.vehicle_state = 0x00;
status_msg.control_mode = 0x01;
status_msg.fault_code = 0x00;
status_msg.error_code = 0x00;
status_msg.battery_voltage = 29.5;
// for (int i = 0; i < 4; ++i)