diff --git a/README.md b/README.md index 02cea19..ad7b2f3 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,15 @@ roslaunch ranger_bringup ranger_teleop_keyboard.launch ## Examples +### 0. enable can control + +```shell + robot->Connect("can0"); + robot->EnableCommandedMode(); +``` + + + ### 1. set motion mode see `ranger_ros/ranger_examples/src/change_the_mode.cpp` for more details diff --git a/ranger_base/include/ranger_base/ranger_params.hpp b/ranger_base/include/ranger_base/ranger_params.hpp index 053c29f..1a0921a 100644 --- a/ranger_base/include/ranger_base/ranger_params.hpp +++ b/ranger_base/include/ranger_base/ranger_params.hpp @@ -19,8 +19,9 @@ struct RangerParams { 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 - // max central angle: pi/2 - static constexpr double max_steer_angle_central = 1.5708; + + static constexpr double max_steer_angle_central = 0.53385; //~= 30.58 degree + static constexpr double max_steer_angle_slide = 0.69813; // 40 degree }; } // namespace westonrobot #endif // RANGER_PARAMS_HPP diff --git a/ranger_base/src/ranger_messenger.cpp b/ranger_base/src/ranger_messenger.cpp index 262658c..c4f1446 100644 --- a/ranger_base/src/ranger_messenger.cpp +++ b/ranger_base/src/ranger_messenger.cpp @@ -54,6 +54,9 @@ void RangerROSMessenger::PublishStateToROS() { 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; + status_msg.current_motion_mode = state.current_motion_mode.motion_mode; + + motion_mode_ = status_msg.current_motion_mode; // linear_velocity, angular_velocity, central steering_angle double l_v = 0.0, a_v = 0.0, phi = 0.0; @@ -62,17 +65,16 @@ void RangerROSMessenger::PublishStateToROS() { double phi_i = -state.motion_state.steering_angle / 180.0 * M_PI; - // ROS_WARN("%f %f %f %f", state.motion_state.linear_velocity, - // state.motion_state.angular_velocity, - // state.motion_state.lateral_velocity, - // state.motion_state.steering_angle); - switch (motion_mode_) { case RangerSetting::MOTION_MODE_ACKERMAN: { l_v = state.motion_state.linear_velocity; - double r = s / std::tan(phi_i) + s; + double r = s / std::tan(std::fabs(phi_i)) + s; phi = ConvertInnerAngleToCentral(phi_i); - a_v = state.motion_state.linear_velocity / r; + if (phi > steer_angle_tolerance) { + a_v = state.motion_state.linear_velocity / r; + } else { + a_v = -state.motion_state.linear_velocity / r; + } x_v = l_v * std::cos(phi); if (l_v >= 0.0) { y_v = l_v * std::sin(phi); @@ -143,13 +145,6 @@ void RangerROSMessenger::GetCurrentMotionCmdForSim(double &linear, void RangerROSMessenger::TwistCmdCallback( const geometry_msgs::Twist::ConstPtr &msg) { double steer_cmd = msg->angular.z; - // TODO: different mode have different limit - if (steer_cmd > RangerParams::max_steer_angle_central) { - steer_cmd = RangerParams::max_steer_angle_central; - } - if (steer_cmd < -RangerParams::max_steer_angle_central) { - steer_cmd = -RangerParams::max_steer_angle_central; - } if (simulated_robot_) { std::lock_guard lg(twist_mutex_); @@ -159,14 +154,27 @@ void RangerROSMessenger::TwistCmdCallback( switch (motion_mode_) { case RangerSetting::MOTION_MODE_ACKERMAN: { + if (steer_cmd > RangerParams::max_steer_angle_central) { + steer_cmd = RangerParams::max_steer_angle_central; + } + if (steer_cmd < -RangerParams::max_steer_angle_central) { + steer_cmd = -RangerParams::max_steer_angle_central; + } + double phi_i = ConvertCentralAngleToInner(steer_cmd); double phi_degree = -(phi_i / M_PI * 180.0); - // std::cout << "set steering angle: " << phi_degree << std::endl; ranger_->SetMotionCommand(msg->linear.x, phi_degree); break; } case RangerSetting::MOTION_MODE_SLIDE: { + if (steer_cmd > RangerParams::max_steer_angle_slide) { + steer_cmd = RangerParams::max_steer_angle_slide; + } + if (steer_cmd < -RangerParams::max_steer_angle_slide) { + steer_cmd = -RangerParams::max_steer_angle_slide; + } + double phi_degree = -(steer_cmd / M_PI * 180.0); ranger_->SetMotionCommand(msg->linear.x, phi_degree); break; diff --git a/ranger_examples/src/change_the_mode.cpp b/ranger_examples/src/change_the_mode.cpp index cfe3c25..f1002fa 100644 --- a/ranger_examples/src/change_the_mode.cpp +++ b/ranger_examples/src/change_the_mode.cpp @@ -11,6 +11,7 @@ #include #include +#include #include "ranger_base/ranger_messenger.hpp" #include "ugv_sdk/ranger_base.hpp" @@ -29,6 +30,12 @@ int main(int argc, char *argv[]) { robot->Connect("can0"); robot->EnableCommandedMode(); + // 0 Arckmann 1 Slide 2 round, 3 Sloping + robot->SetMotionMode(0); + // robot->SetMotionMode(1); + // robot->SetMotionMode(2); + // robot->SetMotionMode(3); + // publish robot state at 50Hz while listening to twist commands ros::Rate rate(50); while (ros::ok()) { diff --git a/ranger_examples/src/control_the_car.cpp b/ranger_examples/src/control_the_car.cpp index cbd7875..22e7f2a 100644 --- a/ranger_examples/src/control_the_car.cpp +++ b/ranger_examples/src/control_the_car.cpp @@ -17,8 +17,12 @@ using namespace westonrobot; -std::shared_ptr robot; +void Ackermann(); +void Slide(); +void Round(); +void Sloping(); +std::shared_ptr robot; int main(int argc, char *argv[]) { // setup ROS node @@ -31,8 +35,10 @@ int main(int argc, char *argv[]) { robot->Connect("can0"); robot->EnableCommandedMode(); - - + Ackermann(); + // Slide(); + // Round(); + // Sloping(); // publish robot state at 50Hz while listening to twist commands ros::Rate rate(50); @@ -42,3 +48,7 @@ int main(int argc, char *argv[]) { } return 0; } +void Ackermann(){} +void Slide(){} +void Round(){} +void Sloping(){} diff --git a/ranger_msgs/msg/RangerStatus.msg b/ranger_msgs/msg/RangerStatus.msg index eb75aed..2e59e3c 100644 --- a/ranger_msgs/msg/RangerStatus.msg +++ b/ranger_msgs/msg/RangerStatus.msg @@ -16,6 +16,7 @@ float64 steering_angle float64 x_linear_vel float64 y_linear_vel float64 motion_radius +uint8 current_motion_mode # base state uint8 vehicle_state @@ -24,7 +25,7 @@ uint16 error_code float64 battery_voltage # motor state -RangerActuatorState[4] actuator_states +RangerActuatorState[8] actuator_states # light state bool light_control_enabled