diff --git a/README.md b/README.md index 8be5546..7cbdef7 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,5 @@ # ROS Packages for Scout Mobile Robot -**Important Note:** Currently we're transitioning the communication protocol from version 1 to version 2. Please check with Weston Robot or AgileX Robotics to confirm which version your robot is using. - -* V1 Protocol: master branch of scout_ros and ugv_sdk -* V2 Protocol: v2.x branch of scout_ros and ugv_sdk - ## Packages This repository contains minimal packages to control the scout robot using ROS. @@ -54,14 +49,13 @@ Nvidia Jeston TX2/Xavier/XavierNX have CAN controller(s) integrated in the main ``` $ cd ~/catkin_ws/src - $ git clone https://github.com/westonrobot/async_port.git - $ git clone -b v2.x https://github.com/westonrobot/ugv_sdk.git - $ git clone -b v2.x https://github.com/westonrobot/scout_ros.git + $ git clone https://github.com/westonrobot/ugv_sdk.git + $ git clone https://github.com/westonrobot/scout_ros.git $ cd .. $ catkin_make ``` -4. Launch ROS nodes +3. Launch ROS nodes * Start the base node for the real robot diff --git a/scout_base/CMakeLists.txt b/scout_base/CMakeLists.txt index 94cc383..bb171a4 100644 --- a/scout_base/CMakeLists.txt +++ b/scout_base/CMakeLists.txt @@ -37,15 +37,10 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include - ${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}) + ${catkin_INCLUDE_DIRS}) add_executable(scout_base_node src/scout_base_node.cpp) -target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES}) +target_link_libraries(scout_base_node ${catkin_LIBRARIES}) ############# ## Install ## diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index e00470c..88c3e2b 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -18,25 +18,40 @@ // #include #include +#include "scout_msgs/ScoutStatus.h" #include "scout_msgs/ScoutLightCmd.h" #include "ugv_sdk/mobile_robot/scout_robot.hpp" namespace westonrobot { +template class ScoutMessenger { public: - explicit ScoutMessenger(ros::NodeHandle *nh); - ScoutMessenger(std::shared_ptr scout, ros::NodeHandle *nh); + ScoutMessenger(std::shared_ptr scout, ros::NodeHandle *nh) + : scout_(scout), nh_(nh) {} - void SetOdometryFrame(std::string frame); - void SetBaseFrame(std::string frame); - void SetOdometryTopicName(std::string name); - void SetSimulationMode(int loop_rate); + void SetOdometryFrame(std::string frame) { odom_frame_ = frame; } + void SetBaseFrame(std::string frame) { base_frame_ = frame; } + void SetOdometryTopicName(std::string name) { odom_topic_name_ = name; } - void Run(); - void Stop(); + void SetSimulationMode(int loop_rate) { + simulated_robot_ = true; + sim_control_rate_ = loop_rate; + } + + void Run() { + SetupSubscription(); + + // publish robot state at 50Hz while listening to twist commands + ros::Rate rate(50); + while (ros::ok()) { + PublishStateToROS(); + ros::spinOnce(); + rate.sleep(); + } + } private: - std::shared_ptr scout_; + std::shared_ptr scout_; ros::NodeHandle *nh_; std::string odom_frame_; @@ -46,7 +61,6 @@ class ScoutMessenger { bool simulated_robot_ = false; int sim_control_rate_ = 50; - std::atomic keep_running_; std::mutex twist_mutex_; geometry_msgs::Twist current_twist_; @@ -67,15 +81,254 @@ class ScoutMessenger { ros::Time last_time_; ros::Time current_time_; - void SetupSubscription(); + void SetupSubscription() { + // odometry publisher + odom_pub_ = nh_->advertise(odom_topic_name_, 50); + status_pub_ = nh_->advertise("/scout_status", 10); - void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); - void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); - void PublishOdometryToROS(double linear, double angular, double dt); + // cmd subscriber + motion_cmd_sub_ = nh_->subscribe( + "/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this); + light_cmd_sub_ = nh_->subscribe( + "/scout_light_control", 5, &ScoutMessenger::LightCmdCallback, this); + } - void PublishStateToROS(); - void PublishSimStateToROS(double linear, double angular); - void GetCurrentMotionCmdForSim(double &linear, double &angular); + void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) { + if (!simulated_robot_) { + scout_->SetMotionCommand(msg->linear.x, msg->angular.z); + } else { + std::lock_guard guard(twist_mutex_); + current_twist_ = *msg.get(); + } + ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); + } + + void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) { + if (!simulated_robot_) { + // if (msg->cmd_ctrl_allowed) { + // ScoutLightCmd cmd; + + // switch (msg->front_mode) { + // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { + // cmd.front_mode = CONST_OFF; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { + // cmd.front_mode = CONST_ON; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { + // cmd.front_mode = BREATH; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { + // cmd.front_mode = CUSTOM; + // cmd.front_custom_value = msg->front_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_->DisableLightControl(); + // } + } else { + std::cout << "simulated robot received light control cmd" << std::endl; + } + } + + void PublishOdometryToROS(double linear, double angular, double dt) { + // perform numerical integration to get an estimation of pose + linear_speed_ = linear; + angular_speed_ = angular; + + double d_x = linear_speed_ * std::cos(theta_) * dt; + double d_y = linear_speed_ * std::sin(theta_) * dt; + double d_theta = angular_speed_ * dt; + + position_x_ += d_x; + position_y_ += d_y; + theta_ += d_theta; + + geometry_msgs::Quaternion odom_quat = + tf::createQuaternionMsgFromYaw(theta_); + + // publish tf transformation + geometry_msgs::TransformStamped tf_msg; + tf_msg.header.stamp = current_time_; + tf_msg.header.frame_id = odom_frame_; + tf_msg.child_frame_id = base_frame_; + + tf_msg.transform.translation.x = position_x_; + tf_msg.transform.translation.y = position_y_; + tf_msg.transform.translation.z = 0.0; + tf_msg.transform.rotation = odom_quat; + + tf_broadcaster_.sendTransform(tf_msg); + + // publish odometry and tf messages + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = current_time_; + odom_msg.header.frame_id = odom_frame_; + odom_msg.child_frame_id = base_frame_; + + odom_msg.pose.pose.position.x = position_x_; + odom_msg.pose.pose.position.y = position_y_; + odom_msg.pose.pose.position.z = 0.0; + odom_msg.pose.pose.orientation = odom_quat; + + odom_msg.twist.twist.linear.x = linear_speed_; + odom_msg.twist.twist.linear.y = 0.0; + odom_msg.twist.twist.angular.z = angular_speed_; + + odom_pub_.publish(odom_msg); + } + + void PublishStateToROS() { + current_time_ = ros::Time::now(); + double dt = (current_time_ - last_time_).toSec(); + + static bool init_run = true; + if (init_run) { + last_time_ = current_time_; + init_run = false; + return; + } + + auto state = scout_->GetRobotState(); + + // publish scout state message + scout_msgs::ScoutStatus status_msg; + + status_msg.header.stamp = current_time_; + + status_msg.linear_velocity = state.motion_state.linear_velocity; + status_msg.angular_velocity = state.motion_state.angular_velocity; + + 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; + + auto actuator = scout_->GetActuatorState(); + + for (int i = 0; i < 4; ++i) { + // actuator_hs_state + uint8_t motor_id = actuator.actuator_hs_state[i].motor_id; + + status_msg.actuator_states[motor_id].rpm = + actuator.actuator_hs_state[i].rpm; + status_msg.actuator_states[motor_id].current = + actuator.actuator_hs_state[i].current; + status_msg.actuator_states[motor_id].pulse_count = + actuator.actuator_hs_state[i].pulse_count; + + // actuator_ls_state + motor_id = actuator.actuator_ls_state[i].motor_id; + + status_msg.actuator_states[motor_id].driver_voltage = + actuator.actuator_ls_state[i].driver_voltage; + status_msg.actuator_states[motor_id].driver_temperature = + actuator.actuator_ls_state[i].driver_temp; + status_msg.actuator_states[motor_id].motor_temperature = + actuator.actuator_ls_state[i].motor_temp; + status_msg.actuator_states[motor_id].driver_state = + actuator.actuator_ls_state[i].driver_state; + } + + status_msg.light_control_enabled = state.light_state.enable_cmd_ctrl; + status_msg.front_light_state.mode = state.light_state.front_light.mode; + status_msg.front_light_state.custom_value = + 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.light_state.rear_light.custom_value; + status_pub_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(state.motion_state.linear_velocity, + state.motion_state.angular_velocity, dt); + + // record time for next integration + last_time_ = current_time_; + } + + void PublishSimStateToROS(double linear, double angular) { + current_time_ = ros::Time::now(); + + double dt = (current_time_ - last_time_).toSec(); + + static bool init_run = true; + if (init_run) { + last_time_ = current_time_; + init_run = false; + return; + } + + // publish scout state message + scout_msgs::ScoutStatus status_msg; + + status_msg.header.stamp = current_time_; + + status_msg.linear_velocity = linear; + status_msg.angular_velocity = angular; + + status_msg.vehicle_state = 0x00; + status_msg.control_mode = 0x01; + status_msg.error_code = 0x00; + status_msg.battery_voltage = 29.5; + + // 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; + // } + + status_msg.light_control_enabled = false; + // status_msg.front_light_state.mode = state.front_light_state.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; status_msg.rear_light_state.custom_value = + // state.front_light_state.custom_value; + + status_pub_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(linear, angular, dt); + + // record time for next integration + last_time_ = current_time_; + } + + // void PublishSimStateToROS(double linear, double angular); + // void GetCurrentMotionCmdForSim(double &linear, double &angular); }; } // namespace westonrobot diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 0e68872..14d1eaa 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -13,18 +13,12 @@ using namespace westonrobot; -std::shared_ptr robot; -std::unique_ptr messenger; -bool keep_run = true; - -void DetachRobot(int signal) { messenger->Stop(); } - int main(int argc, char **argv) { // setup ROS node ros::init(argc, argv, "scout_node"); ros::NodeHandle node(""), private_node("~"); - std::signal(SIGINT, DetachRobot); + // std::signal(SIGINT, DetachRobot); // fetch parameters before connecting to robot std::string port_name; @@ -52,6 +46,9 @@ int main(int argc, char **argv) { ROS_INFO("Robot base: Scout"); } + std::shared_ptr robot; + std::unique_ptr> messenger; + // instantiate a robot object ProtocolDectctor detector; if (detector.Connect(port_name)) { @@ -73,7 +70,8 @@ int main(int argc, char **argv) { } // instantiate a ROS messenger - messenger = std::unique_ptr(new ScoutMessenger(robot, &node)); + messenger = std::unique_ptr>( + new ScoutMessenger(robot, &node)); messenger->SetOdometryFrame(odom_frame); messenger->SetBaseFrame(base_frame); diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp deleted file mode 100644 index fcecaba..0000000 --- a/scout_base/src/scout_messenger.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/* - * scout_messenger.cpp - * - * Created on: Apr 26, 2019 22:14 - * Description: - * - * Copyright (c) 2019 Ruixiang Du (rdu) - */ - -#include "scout_base/scout_messenger.hpp" - -#include - -#include "scout_msgs/ScoutStatus.h" - -namespace westonrobot { -ScoutMessenger::ScoutMessenger(ros::NodeHandle *nh) - : scout_(nullptr), nh_(nh) {} - -ScoutMessenger::ScoutMessenger(std::shared_ptr scout, - ros::NodeHandle *nh) - : scout_(scout), nh_(nh) {} - -void ScoutMessenger::SetOdometryFrame(std::string frame) { - odom_frame_ = frame; -} - -void ScoutMessenger::SetBaseFrame(std::string frame) { base_frame_ = frame; } - -void ScoutMessenger::SetOdometryTopicName(std::string name) { - odom_topic_name_ = name; -} - -void ScoutMessenger::SetSimulationMode(int loop_rate) { - simulated_robot_ = true; - sim_control_rate_ = loop_rate; -} - -void ScoutMessenger::SetupSubscription() { - // odometry publisher - odom_pub_ = nh_->advertise(odom_topic_name_, 50); - status_pub_ = nh_->advertise("/scout_status", 10); - - // cmd subscriber - motion_cmd_sub_ = nh_->subscribe( - "/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this); - light_cmd_sub_ = nh_->subscribe( - "/scout_light_control", 5, &ScoutMessenger::LightCmdCallback, this); -} - -void ScoutMessenger::Run() { - SetupSubscription(); - - // publish robot state at 50Hz while listening to twist commands - keep_running_ = true; - ros::Rate rate(50); - while (keep_running_) { - PublishStateToROS(); - ros::spinOnce(); - rate.sleep(); - } -} - -void ScoutMessenger::Stop() { keep_running_ = false; } - -void ScoutMessenger::TwistCmdCallback( - const geometry_msgs::Twist::ConstPtr &msg) { - if (!simulated_robot_) { - scout_->SetMotionCommand(msg->linear.x, msg->angular.z); - } else { - std::lock_guard guard(twist_mutex_); - current_twist_ = *msg.get(); - } - ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); -} - -void ScoutMessenger::GetCurrentMotionCmdForSim(double &linear, - double &angular) { - std::lock_guard guard(twist_mutex_); - linear = current_twist_.linear.x; - angular = current_twist_.angular.z; -} - -void ScoutMessenger::LightCmdCallback( - const scout_msgs::ScoutLightCmd::ConstPtr &msg) { - if (!simulated_robot_) { - // if (msg->cmd_ctrl_allowed) { - // ScoutLightCmd cmd; - - // switch (msg->front_mode) { - // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { - // cmd.front_mode = CONST_OFF; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { - // cmd.front_mode = CONST_ON; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { - // cmd.front_mode = BREATH; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { - // cmd.front_mode = CUSTOM; - // cmd.front_custom_value = msg->front_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_->DisableLightControl(); - // } - } else { - std::cout << "simulated robot received light control cmd" << std::endl; - } -} - -void ScoutMessenger::PublishStateToROS() { - current_time_ = ros::Time::now(); - double dt = (current_time_ - last_time_).toSec(); - - static bool init_run = true; - if (init_run) { - last_time_ = current_time_; - init_run = false; - return; - } - - auto state = scout_->GetRobotState(); - - // publish scout state message - scout_msgs::ScoutStatus status_msg; - - status_msg.header.stamp = current_time_; - - status_msg.linear_velocity = state.motion_state.linear_velocity; - status_msg.angular_velocity = state.motion_state.angular_velocity; - - 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; - - auto actuator = scout_->GetActuatorState(); - - for (int i = 0; i < 4; ++i) { - // actuator_hs_state - uint8_t motor_id = actuator.actuator_hs_state[i].motor_id; - - status_msg.actuator_states[motor_id].rpm = - actuator.actuator_hs_state[i].rpm; - status_msg.actuator_states[motor_id].current = - actuator.actuator_hs_state[i].current; - status_msg.actuator_states[motor_id].pulse_count = - actuator.actuator_hs_state[i].pulse_count; - - // actuator_ls_state - motor_id = actuator.actuator_ls_state[i].motor_id; - - status_msg.actuator_states[motor_id].driver_voltage = - actuator.actuator_ls_state[i].driver_voltage; - status_msg.actuator_states[motor_id].driver_temperature = - actuator.actuator_ls_state[i].driver_temp; - status_msg.actuator_states[motor_id].motor_temperature = - actuator.actuator_ls_state[i].motor_temp; - status_msg.actuator_states[motor_id].driver_state = - actuator.actuator_ls_state[i].driver_state; - } - - status_msg.light_control_enabled = state.light_state.enable_cmd_ctrl; - status_msg.front_light_state.mode = state.light_state.front_light.mode; - status_msg.front_light_state.custom_value = - 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.light_state.rear_light.custom_value; - status_pub_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(state.motion_state.linear_velocity, - state.motion_state.angular_velocity, dt); - - // record time for next integration - last_time_ = current_time_; -} - -void ScoutMessenger::PublishSimStateToROS(double linear, double angular) { - current_time_ = ros::Time::now(); - - double dt = (current_time_ - last_time_).toSec(); - - static bool init_run = true; - if (init_run) { - last_time_ = current_time_; - init_run = false; - return; - } - - // publish scout state message - scout_msgs::ScoutStatus status_msg; - - status_msg.header.stamp = current_time_; - - status_msg.linear_velocity = linear; - status_msg.angular_velocity = angular; - - status_msg.vehicle_state = 0x00; - status_msg.control_mode = 0x01; - status_msg.error_code = 0x00; - status_msg.battery_voltage = 29.5; - - // 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; - // } - - status_msg.light_control_enabled = false; - // status_msg.front_light_state.mode = state.front_light_state.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; status_msg.rear_light_state.custom_value = - // state.front_light_state.custom_value; - - status_pub_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(linear, angular, dt); - - // record time for next integration - last_time_ = current_time_; -} - -void ScoutMessenger::PublishOdometryToROS(double linear, double angular, - double dt) { - // perform numerical integration to get an estimation of pose - linear_speed_ = linear; - angular_speed_ = angular; - - double d_x = linear_speed_ * std::cos(theta_) * dt; - double d_y = linear_speed_ * std::sin(theta_) * dt; - double d_theta = angular_speed_ * dt; - - position_x_ += d_x; - position_y_ += d_y; - theta_ += d_theta; - - geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_); - - // publish tf transformation - geometry_msgs::TransformStamped tf_msg; - tf_msg.header.stamp = current_time_; - tf_msg.header.frame_id = odom_frame_; - tf_msg.child_frame_id = base_frame_; - - tf_msg.transform.translation.x = position_x_; - tf_msg.transform.translation.y = position_y_; - tf_msg.transform.translation.z = 0.0; - tf_msg.transform.rotation = odom_quat; - - tf_broadcaster_.sendTransform(tf_msg); - - // publish odometry and tf messages - nav_msgs::Odometry odom_msg; - odom_msg.header.stamp = current_time_; - odom_msg.header.frame_id = odom_frame_; - odom_msg.child_frame_id = base_frame_; - - odom_msg.pose.pose.position.x = position_x_; - odom_msg.pose.pose.position.y = position_y_; - odom_msg.pose.pose.position.z = 0.0; - odom_msg.pose.pose.orientation = odom_quat; - - odom_msg.twist.twist.linear.x = linear_speed_; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.angular.z = angular_speed_; - - odom_pub_.publish(odom_msg); -} -} // namespace westonrobot \ No newline at end of file