From 910a1e08f8fa01d12d3f0f32b0d364645c785c0a Mon Sep 17 00:00:00 2001 From: pd-tan Date: Fri, 18 Sep 2020 09:58:32 +0000 Subject: [PATCH] Pd dev --- .../include/scout_base/scout_params.hpp | 32 +- scout_base/launch/scout_base.launch | 6 +- scout_base/src/scout_messenger.cpp | 374 ++++++++++-------- scout_bringup/launch/scout_minimal.launch | 12 +- .../launch/scout_minimal_uart.launch | 11 +- .../launch/scout_teleop_keyboard.launch | 3 + 6 files changed, 232 insertions(+), 206 deletions(-) diff --git a/scout_base/include/scout_base/scout_params.hpp b/scout_base/include/scout_base/scout_params.hpp index 1f0dfc9..3c1f55d 100644 --- a/scout_base/include/scout_base/scout_params.hpp +++ b/scout_base/include/scout_base/scout_params.hpp @@ -5,31 +5,29 @@ * Description: * * Copyright (c) 2020 Ruixiang Du (rdu) - */ + */ #ifndef SCOUT_PARAMS_HPP #define SCOUT_PARAMS_HPP -#include - namespace westonrobot { -struct ScoutParams -{ - /* Scout Parameters */ - static constexpr double max_steer_angle = 30.0; // in degree + struct ScoutParams + { + /* Scout Parameters */ + static constexpr double max_steer_angle = 30.0; // in degree - static constexpr double track = 0.58306; // in meter (left & right wheel distance) - static constexpr double wheelbase = 0.498; // in meter (front & rear wheel distance) - static constexpr double wheel_radius = 0.165; // in meter + static constexpr double track = 0.58306; // in meter (left & right wheel distance) + static constexpr double wheelbase = 0.498; // in meter (front & rear wheel distance) + static constexpr double wheel_radius = 0.165; // in meter - // from user manual v1.2.8 P18 - // max linear velocity: 1.5 m/s - // max angular velocity: 0.7853 rad/s - static constexpr double max_linear_speed = 1.5; // in m/s - static constexpr double max_angular_speed = 0.7853; // in rad/s - static constexpr double max_speed_cmd = 10.0; // in rad/s -}; + // from user manual v1.2.8 P18 + // max linear velocity: 1.5 m/s + // max angular velocity: 0.7853 rad/s + static constexpr double max_linear_speed = 1.5; // in m/s + static constexpr double max_angular_speed = 0.7853; // in rad/s + static constexpr double max_speed_cmd = 10.0; // in rad/s + }; } // namespace westonrobot #endif /* SCOUT_PARAMS_HPP */ diff --git a/scout_base/launch/scout_base.launch b/scout_base/launch/scout_base.launch index 558474b..dd5bb55 100644 --- a/scout_base/launch/scout_base.launch +++ b/scout_base/launch/scout_base.launch @@ -11,16 +11,14 @@ --> - - + - - + diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 26493ed..946b77a 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -13,244 +13,270 @@ #include "scout_msgs/ScoutStatus.h" +namespace westonrobot +{ + ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) + : scout_(nullptr), nh_(nh) {} -namespace westonrobot { -ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) - : scout_(nullptr), nh_(nh) {} + ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) + : scout_(scout), nh_(nh) {} + void ScoutROSMessenger::SetupSubscription() + { + // odometry publisher + odom_publisher_ = nh_->advertise(odom_topic_name_, 50); + status_publisher_ = + nh_->advertise("/scout_status", 10); -ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) - : scout_(scout), nh_(nh) {} - -void ScoutROSMessenger::SetupSubscription() { - // odometry publisher - odom_publisher_ = nh_->advertise(odom_topic_name_, 50); - status_publisher_ = - nh_->advertise("/scout_status", 10); - - // cmd subscriber - motion_cmd_subscriber_ = nh_->subscribe( - "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); - light_cmd_subscriber_ = nh_->subscribe( - "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); -} - -void ScoutROSMessenger::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(); + // cmd subscriber + motion_cmd_subscriber_ = nh_->subscribe( + "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); + light_cmd_subscriber_ = nh_->subscribe( + "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); } - // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); -} -void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, - double &angular) { - std::lock_guard guard(twist_mutex_); - linear = current_twist_.linear.x; - angular = current_twist_.angular.z; -} + void ScoutROSMessenger::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 ScoutROSMessenger::LightCmdCallback( - const scout_msgs::ScoutLightCmd::ConstPtr &msg) { - if (!simulated_robot_) { - if (msg->enable_cmd_light_control) { - ScoutLightCmd cmd; + void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, + double &angular) + { + std::lock_guard guard(twist_mutex_); + linear = current_twist_.linear.x; + angular = current_twist_.angular.z; + } - switch (msg->front_mode) { - case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { + void ScoutROSMessenger::LightCmdCallback( + const scout_msgs::ScoutLightCmd::ConstPtr &msg) + { + if (!simulated_robot_) + { + if (msg->enable_cmd_light_control) + { + ScoutLightCmd cmd; + + switch (msg->front_mode) + { + case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: + { cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF; break; } - case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { + case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: + { cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; break; } - case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { + case scout_msgs::ScoutLightCmd::LIGHT_BREATH: + { cmd.front_mode = ScoutLightCmd::LightMode::BREATH; break; } - case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { + case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: + { cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.front_custom_value = msg->front_custom_value; break; } - } + } - switch (msg->rear_mode) { - case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { + 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: { + case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: + { cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON; break; } - case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { + case scout_msgs::ScoutLightCmd::LIGHT_BREATH: + { cmd.rear_mode = ScoutLightCmd::LightMode::BREATH; break; } - case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { + case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: + { cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.rear_custom_value = msg->rear_custom_value; break; } + } + + scout_->SetLightCommand(cmd); + } + else + { + scout_->DisableLightCmdControl(); } - - scout_->SetLightCommand(cmd); - } else { - scout_->DisableLightCmdControl(); } - } else { - std::cout << "simulated robot received light control cmd" << std::endl; + else + { + std::cout << "simulated robot received light control cmd" << std::endl; + } } -} + void ScoutROSMessenger::PublishStateToROS() + { + current_time_ = ros::Time::now(); + double dt = (current_time_ - last_time_).toSec(); -void ScoutROSMessenger::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; + } - static bool init_run = true; - if (init_run) { + auto state = scout_->GetScoutState(); + + // publish scout state message + scout_msgs::ScoutStatus status_msg; + + status_msg.header.stamp = current_time_; + + status_msg.linear_velocity = state.linear_velocity; + status_msg.angular_velocity = 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; + + 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 = state.light_control_enabled; + 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_publisher_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt); + + // record time for next integration last_time_ = current_time_; - init_run = false; - return; } - auto state = scout_->GetScoutState(); + void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) + { + current_time_ = ros::Time::now(); - // publish scout state message - scout_msgs::ScoutStatus status_msg; + double dt = (current_time_ - last_time_).toSec(); - status_msg.header.stamp = current_time_; + static bool init_run = true; + if (init_run) + { + last_time_ = current_time_; + init_run = false; + return; + } - status_msg.linear_velocity = state.linear_velocity; - status_msg.angular_velocity = state.angular_velocity; + // publish scout state message + scout_msgs::ScoutStatus status_msg; - 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.header.stamp = current_time_; - 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.linear_velocity = linear; + status_msg.angular_velocity = angular; - status_msg.light_control_enabled = state.light_control_enabled; - 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_publisher_.publish(status_msg); + status_msg.base_state = 0x00; + status_msg.control_mode = 0x01; + status_msg.fault_code = 0x00; + status_msg.battery_voltage = 29.5; - // publish odometry and tf - PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt); + // 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; + // } - // record time for next integration - last_time_ = current_time_; -} + 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; -void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { - current_time_ = ros::Time::now(); + status_publisher_.publish(status_msg); - double dt = (current_time_ - last_time_).toSec(); + // publish odometry and tf + PublishOdometryToROS(linear, angular, dt); - static bool init_run = true; - if (init_run) { + // record time for next integration last_time_ = current_time_; - init_run = false; - return; } - - // publish scout state message - scout_msgs::ScoutStatus status_msg; + void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, + double dt) + { + // perform numerical integration to get an estimation of pose + linear_speed_ = linear; + angular_speed_ = angular; - status_msg.header.stamp = current_time_; + double d_x = linear_speed_ * std::cos(theta_) * dt; + double d_y = linear_speed_ * std::sin(theta_) * dt; + double d_theta = angular_speed_ * dt; - status_msg.linear_velocity = linear; - status_msg.angular_velocity = angular; + position_x_ += d_x; + position_y_ += d_y; + theta_ += d_theta; - status_msg.base_state = 0x00; - status_msg.control_mode = 0x01; - status_msg.fault_code = 0x00; - status_msg.battery_voltage = 29.5; + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_); - // 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; - // } + // 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_; - 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; + 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; - status_publisher_.publish(status_msg); + tf_broadcaster_.sendTransform(tf_msg); - // publish odometry and tf - PublishOdometryToROS(linear, angular, dt); + // publish odometry and tf messages + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = current_time_; + odom_msg.header.frame_id = "testes"; + odom_msg.child_frame_id = base_frame_; - // record time for next integration - last_time_ = current_time_; -} + 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; -void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, - double dt) { - // perform numerical integration to get an estimation of pose - linear_speed_ = linear; - angular_speed_ = angular; + odom_msg.twist.twist.linear.x = linear_speed_; + odom_msg.twist.twist.linear.y = 0.0; + odom_msg.twist.twist.angular.z = angular_speed_; - 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 = "testes"; - 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_publisher_.publish(odom_msg); -} -} // namespace westonrobot \ No newline at end of file + odom_publisher_.publish(odom_msg); + } +} // namespace westonrobot \ No newline at end of file diff --git a/scout_bringup/launch/scout_minimal.launch b/scout_bringup/launch/scout_minimal.launch index f19cdae..21e21a4 100644 --- a/scout_bringup/launch/scout_minimal.launch +++ b/scout_bringup/launch/scout_minimal.launch @@ -2,17 +2,17 @@ - - + + - + - - - + + + \ No newline at end of file diff --git a/scout_bringup/launch/scout_minimal_uart.launch b/scout_bringup/launch/scout_minimal_uart.launch index 548341d..e29fa48 100644 --- a/scout_bringup/launch/scout_minimal_uart.launch +++ b/scout_bringup/launch/scout_minimal_uart.launch @@ -2,17 +2,18 @@ - - + + - + - - + + + \ No newline at end of file diff --git a/scout_bringup/launch/scout_teleop_keyboard.launch b/scout_bringup/launch/scout_teleop_keyboard.launch index 1eedb55..73381f1 100644 --- a/scout_bringup/launch/scout_teleop_keyboard.launch +++ b/scout_bringup/launch/scout_teleop_keyboard.launch @@ -1,4 +1,7 @@ + + +