From 840739d4fe6b4b1b029221ce9504a67143789a23 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Thu, 1 Apr 2021 15:48:53 +0800 Subject: [PATCH] tested work with both scout_base_node and scout_messenger --- scout_base/CMakeLists.txt | 11 +- .../include/scout_base/scout_messenger.hpp | 2 +- scout_base/src/scout_base_node.cpp | 195 ++++-------------- scout_base/src/scout_messenger.cpp | 109 +++++----- 4 files changed, 105 insertions(+), 212 deletions(-) diff --git a/scout_base/CMakeLists.txt b/scout_base/CMakeLists.txt index 28d1b2b..94cc383 100644 --- a/scout_base/CMakeLists.txt +++ b/scout_base/CMakeLists.txt @@ -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 ## diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index b78bf9a..45c932e 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -18,7 +18,7 @@ #include #include "scout_msgs/ScoutLightCmd.h" -#include "ugv_sdk/scout/scout_base.hpp" +#include "ugv_sdk/scout_base.hpp" namespace westonrobot { diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 4b965f6..8567f16 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -5,175 +5,58 @@ #include #include -#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 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("port_name", device_name, std::string("can0")); + // instantiate a robot object robot = std::make_shared(); - robot->Connect(device_name); - robot->EnableCommandedMode(); + ScoutROSMessenger messenger(robot.get(), &node); - // publisher - ros::Publisher state_pub = node.advertise( - "/scout_motion_state", 10); + // 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")); - // 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(); - 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; } \ No newline at end of file diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 946b77a..23fb1a6 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -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)