From 9a263392fcb803fb9435fdbd7f6debc4c6c6d4ad Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 28 Aug 2020 17:07:00 +0800 Subject: [PATCH 1/4] removed scout_base_sim_node.cpp --- scout_base/src/scout_base_sim_node.cpp | 47 -------------------------- 1 file changed, 47 deletions(-) delete mode 100644 scout_base/src/scout_base_sim_node.cpp diff --git a/scout_base/src/scout_base_sim_node.cpp b/scout_base/src/scout_base_sim_node.cpp deleted file mode 100644 index acafbd9..0000000 --- a/scout_base/src/scout_base_sim_node.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include - -#include -#include -#include -#include - -#include "wrp_sdk/platforms/scout/scout_base.hpp" -#include "scout_base/scout_messenger.hpp" - -using namespace westonrobot; - -int main(int argc, char **argv) -{ - // setup ROS node - ros::init(argc, argv, "scout_odom"); - ros::NodeHandle node(""), private_node("~"); - - // instantiate a robot object - ScoutBase robot; - ScoutROSMessenger messenger(&robot, &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_, true); - private_node.param("control_rate", messenger.sim_control_rate_, 50); - - // no connection for simulated robot - // setup ROS subscription - messenger.SetupSubscription(); - - // publish robot state at 50Hz while listening to twist commands - double linear, angular; - ros::Rate rate_50hz(50); // 50Hz - while (true) - { - messenger.GetCurrentMotionCmdForSim(linear, angular); - messenger.PublishSimStateToROS(linear, angular); - ros::spinOnce(); - rate_50hz.sleep(); - } - - return 0; -} \ No newline at end of file From 2d70612bc047c18762d690b1541d8f6fd1974ca6 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 28 Aug 2020 17:47:54 +0800 Subject: [PATCH 2/4] updated odom calc for simulator --- scout_base/src/scout_messenger.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 02870b0..fd8594f 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -179,7 +179,16 @@ void ScoutROSMessenger::PublishStateToROS() void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { current_time_ = ros::Time::now(); - double dt = 1.0 / sim_control_rate_; + + 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; @@ -211,6 +220,9 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) // publish odometry and tf PublishOdometryToROS(linear, angular, dt); + + // record time for next integration + last_time_ = current_time_; } void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt) From f5370e2f3e37ce178c033a6940296ebffc68695d Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 28 Aug 2020 18:55:56 +0800 Subject: [PATCH 3/4] saved work --- scout_base/src/scout_messenger.cpp | 441 ++++++++++++++--------------- 1 file changed, 210 insertions(+), 231 deletions(-) diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index fd8594f..2f267f4 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -1,9 +1,9 @@ -/* +/* * scout_messenger.cpp - * + * * Created on: Apr 26, 2019 22:14 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) */ @@ -13,262 +13,241 @@ #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_frame_, 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); } -ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) : scout_(scout), nh_(nh) -{ -} - -void ScoutROSMessenger::SetupSubscription() -{ - // odometry publisher - odom_publisher_ = nh_->advertise(odom_frame_, 50); - status_publisher_ = nh_->advertise("/scout_status", 10); - - // cmd subscriber - motion_cmd_subscriber_ = nh_->subscribe("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel” - 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(); - } - // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); -} - -void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular) -{ +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_); - linear = current_twist_.linear.x; - angular = current_twist_.angular.z; + 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: - { - cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: - { - cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_BREATH: - { - cmd.front_mode = ScoutLightCmd::LightMode::BREATH; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: - { - cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM; - cmd.front_custom_value = msg->front_custom_value; - break; - } - } +void ScoutROSMessenger::LightCmdCallback( + const scout_msgs::ScoutLightCmd::ConstPtr &msg) { + if (!simulated_robot_) { + if (msg->enable_cmd_light_control) { + ScoutLightCmd cmd; - 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; - } - } - - scout_->SetLightCommand(cmd); + switch (msg->front_mode) { + case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { + cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF; + break; } - else - { - scout_->DisableLightCmdControl(); + case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { + cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; + break; } + case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { + cmd.front_mode = ScoutLightCmd::LightMode::BREATH; + break; + } + 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: { + 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; + } + } + + 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; - } - - 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 + static bool init_run = true; + if (init_run) { last_time_ = current_time_; + init_run = false; + return; + } + + 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_; } -void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) -{ - current_time_ = ros::Time::now(); +void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { + current_time_ = ros::Time::now(); - double dt = (current_time_ - last_time_).toSec(); + 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.base_state = 0x00; - status_msg.control_mode = 0x01; - status_msg.fault_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_publisher_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(linear, angular, dt); - - // record time for next integration + 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.base_state = 0x00; + status_msg.control_mode = 0x01; + status_msg.fault_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_publisher_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(linear, angular, dt); + + // record time for next integration + last_time_ = current_time_; } -void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt) -{ - // perform numerical integration to get an estimation of pose - linear_speed_ = linear; - angular_speed_ = angular; +void ScoutROSMessenger::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; + 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; + position_x_ += d_x; + position_y_ += d_y; + theta_ += d_theta; - geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(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_; + // 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_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); + 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_; + // 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.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_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); + odom_publisher_.publish(odom_msg); } -} // namespace westonrobot \ No newline at end of file +} // namespace westonrobot \ No newline at end of file From 1da7e9db5e70a37ed0b4c911a2dcb7d06c818671 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 31 Aug 2020 18:51:01 +0800 Subject: [PATCH 4/4] removed unused urdf --- scout_base/urdf/agilex_scout.urdf | 72 ------------------------------- 1 file changed, 72 deletions(-) delete mode 100644 scout_base/urdf/agilex_scout.urdf diff --git a/scout_base/urdf/agilex_scout.urdf b/scout_base/urdf/agilex_scout.urdf deleted file mode 100644 index 6d1caf4..0000000 --- a/scout_base/urdf/agilex_scout.urdf +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -