From 08ea40365817b0b8d6e95eeeecad48dc4df2327c Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Thu, 1 Apr 2021 11:29:03 +0800 Subject: [PATCH] updated scout_base_node --- scout_base/src/scout_base_node.cpp | 216 +++++++++++++++++++++-------- 1 file changed, 162 insertions(+), 54 deletions(-) diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 3aa1a4c..dc35e6d 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -5,72 +5,180 @@ #include #include -#include "ugv_sdk/scout/scout_base.hpp" -#include "scout_base/scout_messenger.hpp" +#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" using namespace westonrobot; +typedef enum { + SYSTEM_MSG = 0, + MOTION_MSG, + LIGHT_MSG, + RC_MSG, + ACTUATOR_MSG +} MsgsType; + std::shared_ptr robot; -void DetachRobot(int signal) { - robot->Disconnect(); - robot->Terminate(); +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_odom"); - ros::NodeHandle node(""), private_node("~"); + ros::init(argc, argv, "scout_node"); + ros::NodeHandle node; - std::signal(SIGINT, DetachRobot); - - // check wether controlling scout mini - bool is_scout_mini = false; - private_node.param("is_scout_mini", is_scout_mini, false); - std::cout << "Working as scout mini: " << is_scout_mini << std::endl; - - // instantiate a robot object - robot = std::make_shared(is_scout_mini); - ScoutROSMessenger messenger(robot.get(), &node); - - // fetch parameters before connecting to robot - std::string port_name; - private_node.param("port_name", port_name, std::string("can0")); - private_node.param("odom_frame", messenger.odom_frame_, - std::string("odom")); - private_node.param("base_frame", messenger.base_frame_, - std::string("base_link")); - private_node.param("simulated_robot", messenger.simulated_robot_, - false); - private_node.param("control_rate", messenger.sim_control_rate_, 50); - private_node.param("odom_topic_name", messenger.odom_topic_name_, - std::string("odom")); - - if (!messenger.simulated_robot_) { - // connect to robot and setup ROS subscription - if (port_name.find("can") != std::string::npos) { - robot->Connect(port_name); - ROS_INFO("Using CAN bus to talk with the robot"); - } else { - robot->Connect(port_name, 115200); - ROS_INFO("Using UART to talk with the robot"); - } + // connect robot device + std::string device_name; + if (argc == 2) { + device_name = {argv[1]}; + ROS_INFO("Specified CAN: %s", device_name.c_str()); + } else { + ROS_INFO("Please specified CAN port name"); + return -1; } - 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); - } + robot = std::make_shared(); + robot->Connect(device_name); + + // publisher + ros::Publisher state_pub = node.advertise( + "/scout_motion_state", 10); + + // subscriber + ros::Subscriber motion_cmd_sub = + node.subscribe("/cmd_vel", 5, ControlCmdCallback); + ros::Subscriber light_cmd_sub = node.subscribe( + "/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(); - rate.sleep(); + loop_rate.sleep(); } - - return 0; } \ No newline at end of file