From 1942fdccb19b6aa3cdde80216ef11f26725190d4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 9 Sep 2021 09:26:47 +0800 Subject: [PATCH] Finish rename of nonholonomic to ackermann, make ackermann vehicles standardized with RMF stack (#49) * Nonholonomic vehicles now report their state Signed-off-by: Luca Della Vedova * Cleanup and change nonholonomic to ackermann Signed-off-by: Luca Della Vedova * Minor cleanup, style Signed-off-by: Luca Della Vedova * Address feedback Signed-off-by: Luca Della Vedova Co-authored-by: ddengster Signed-off-by: Luca Della Vedova --- .../rmf_robot_sim_common/slotcar_common.hpp | 48 ++--- rmf_robot_sim_common/src/slotcar_common.cpp | 186 ++++++++---------- rmf_robot_sim_gazebo_plugins/src/slotcar.cpp | 4 +- .../src/slotcar.cpp | 42 +--- 4 files changed, 121 insertions(+), 159 deletions(-) diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp index 5318700..36db966 100644 --- a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp +++ b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp @@ -37,9 +37,9 @@ namespace rmf_robot_sim_common { // TODO migrate ign-math-eigen conversions when upgrading to ign-math5 //3rd coordinate is yaw -struct NonHolonomicTrajectory +struct AckermannTrajectory { - NonHolonomicTrajectory(const Eigen::Vector2d& _x0, const Eigen::Vector2d& _x1, + AckermannTrajectory(const Eigen::Vector2d& _x0, const Eigen::Vector2d& _x1, const Eigen::Vector2d& _v1 = Eigen::Vector2d(0, 0), bool _turning = false) : x0(_x0), x1(_x1), @@ -112,13 +112,13 @@ inline Eigen::Isometry3d convert_pose(const IgnPoseT& _pose) return tf; } -typedef struct TrajectoryPoint +struct TrajectoryPoint { Eigen::Vector3d pos; Eigen::Quaterniond quat; TrajectoryPoint(const Eigen::Vector3d& _pos, const Eigen::Quaterniond& _quat) : pos(_pos), quat(_quat) {} -} TrajectoryPoint; +}; // steering type constants enum class SteeringType @@ -130,6 +130,13 @@ enum class SteeringType class SlotcarCommon { public: + struct UpdateResult + { + double v = 0.0; // Target displacement in X (forward) + double w = 0.0; // Target displacement in yaw + double speed = 0.0; // Target speed + }; + SlotcarCommon(); rclcpp::Logger logger() const; @@ -143,13 +150,10 @@ class SlotcarCommon void init_ros_node(const rclcpp::Node::SharedPtr node); - std::pair update(const Eigen::Isometry3d& pose, + UpdateResult update(const Eigen::Isometry3d& pose, const std::vector& obstacle_positions, const double time); - std::pair update_nonholonomic(Eigen::Isometry3d& pose, - double& target_linear_velocity); - bool emergency_stop(const std::vector& obstacle_positions, const Eigen::Vector3d& current_heading); @@ -164,20 +168,10 @@ class SlotcarCommon const std::pair& displacements, const double dt) const; - std::array calculate_model_control_signals( - const std::array& curr_velocities, - const std::pair& displacements, - const double dt, - const double target_linear_velocity = 0.0) const; - void charge_state_cb(const std::string& name, bool selected); void publish_robot_state(const double time); - SteeringType get_steering_type() const; - - bool is_ackermann_steered() const; - private: // Paramters needed for power dissipation and charging calculations // Default values may be overriden using values from sdf file @@ -218,15 +212,14 @@ class SlotcarCommon std::vector trajectory; std::size_t _traj_wp_idx = 0; - std::vector nonholonomic_trajectory; - std::size_t _nonholonomic_traj_idx = 0; + std::vector ackermann_trajectory; + std::size_t _ackermann_traj_idx = 0; rmf_fleet_msgs::msg::PauseRequest pause_request; std::vector _hold_times; std::mutex _mutex; - std::mutex _ackmann_path_req_mutex; std::string _model_name; bool _emergency_stop = false; @@ -317,7 +310,10 @@ class SlotcarCommon void path_request_cb(const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg); - void ackmann_path_request_cb( + void handle_diff_drive_path_request( + const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg); + + void handle_ackermann_path_request( const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg); void pause_request_cb(const rmf_fleet_msgs::msg::PauseRequest::SharedPtr msg); @@ -326,6 +322,14 @@ class SlotcarCommon void map_cb(const rmf_building_map_msgs::msg::BuildingMap::SharedPtr msg); + UpdateResult update_diff_drive( + const std::vector& obstacle_positions, + const double time); + + UpdateResult update_ackermann( + const std::vector& obstacle_positions, + const double time); + bool near_charger(const Eigen::Isometry3d& pose) const; double compute_charge(const double run_time) const; diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index d7cfa8e..c0af2c3 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -149,16 +149,6 @@ void SlotcarCommon::init_ros_node(const rclcpp::Node::SharedPtr node) "/robot_mode_requests", 10, std::bind(&SlotcarCommon::mode_request_cb, this, std::placeholders::_1)); - - if (is_ackermann_steered()) - { - _traj_sub = - _ros_node->create_subscription( - "/ackmann_path_requests", - 10, - std::bind(&SlotcarCommon::ackmann_path_request_cb, this, - std::placeholders::_1)); - } } bool SlotcarCommon::path_request_valid( @@ -192,7 +182,23 @@ void SlotcarCommon::path_request_cb( { if (path_request_valid(msg) == false) return; + std::lock_guard lock(_mutex); + switch (this->_steering_type) + { + case SteeringType::DIFF_DRIVE: + handle_diff_drive_path_request(msg); + break; + case SteeringType::ACKERMANN: + handle_ackermann_path_request(msg); + break; + default: + break; + } +} +void SlotcarCommon::handle_diff_drive_path_request( + const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg) +{ const auto old_path = _remaining_path; RCLCPP_INFO( @@ -200,7 +206,6 @@ void SlotcarCommon::path_request_cb( "%s received a path request with %d waypoints", _model_name.c_str(), (int)msg->path.size()); - std::lock_guard lock(_mutex); // Reset this if we aren't at the final waypoint trajectory.resize(msg->path.size()); _hold_times.resize(msg->path.size()); @@ -253,31 +258,26 @@ void SlotcarCommon::path_request_cb( } } -void SlotcarCommon::ackmann_path_request_cb( +void SlotcarCommon::handle_ackermann_path_request( const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg) { - if (path_request_valid(msg) == false) - return; - if (model_name() != msg->robot_name) - return; // yaw is ignored - std::lock_guard lock(_ackmann_path_req_mutex); double min_turning_radius = _min_turning_radius; if (min_turning_radius < 0.0) min_turning_radius = _nominal_drive_speed / _nominal_turn_speed; - nonholonomic_trajectory.clear(); - _nonholonomic_traj_idx = 0; + ackermann_trajectory.clear(); + _ackermann_traj_idx = 0; auto& locations = msg->path; if (locations.size() < 2) return; // add 1st trajectory - NonHolonomicTrajectory traj( + AckermannTrajectory traj( Eigen::Vector2d(locations[0].x, locations[0].y), Eigen::Vector2d(locations[1].x, locations[1].y)); - this->nonholonomic_trajectory.push_back(traj); + this->ackermann_trajectory.push_back(traj); for (uint i = 2; i < locations.size(); ++i) { @@ -336,18 +336,18 @@ void SlotcarCommon::ackmann_path_request_cb( cp /= (wp1_to_wp0_len * wp1_to_wp2_len); if (std::abs(cp) < 0.05 || !has_runway) { - NonHolonomicTrajectory sp2( + AckermannTrajectory sp2( Eigen::Vector2d(wp[1].x(), wp[1].y()), Eigen::Vector2d(wp[2].x(), wp[2].y())); - NonHolonomicTrajectory& last_traj = this->nonholonomic_trajectory.back(); + AckermannTrajectory& last_traj = this->ackermann_trajectory.back(); last_traj.v1 = sp2.v0; - this->nonholonomic_trajectory.push_back(sp2); + this->ackermann_trajectory.push_back(sp2); } else { - NonHolonomicTrajectory& last_traj = this->nonholonomic_trajectory.back(); + AckermannTrajectory& last_traj = this->ackermann_trajectory.back(); // bend, build an intermediate spline using turn rate. Eigen::Vector2d tangent0 = wp[1] + tangent_length * wp1_to_wp0_norm; @@ -357,7 +357,7 @@ void SlotcarCommon::ackmann_path_request_cb( last_traj.x1 = Eigen::Vector2d(tangent0.x(), tangent0.y()); last_traj.v1 = last_traj.v0; - NonHolonomicTrajectory turn_traj( + AckermannTrajectory turn_traj( Eigen::Vector2d(tangent0.x(), tangent0.y()), Eigen::Vector2d(tangent1.x(), tangent1.y()), Eigen::Vector2d(0, 0), @@ -365,18 +365,18 @@ void SlotcarCommon::ackmann_path_request_cb( turn_traj.v0 = -wp1_to_wp0_norm; turn_traj.v1 = wp1_to_wp2_norm; - NonHolonomicTrajectory end_traj( + AckermannTrajectory end_traj( Eigen::Vector2d(tangent1.x(), tangent1.y()), Eigen::Vector2d(wp[2].x(), wp[2].y())); end_traj.v0 = wp1_to_wp2_norm; end_traj.v1 = wp1_to_wp2_norm; - this->nonholonomic_trajectory.push_back(turn_traj); - this->nonholonomic_trajectory.push_back(end_traj); + this->ackermann_trajectory.push_back(turn_traj); + this->ackermann_trajectory.push_back(end_traj); } } - NonHolonomicTrajectory& last_traj = this->nonholonomic_trajectory.back(); + AckermannTrajectory& last_traj = this->ackermann_trajectory.back(); last_traj.v1 = last_traj.v0; } @@ -413,16 +413,6 @@ std::array SlotcarCommon::calculate_control_signals( return std::array{v_target, w_target}; } -std::array SlotcarCommon::calculate_model_control_signals( - const std::array& curr_velocities, - const std::pair& displacements, - const double dt, - const double target_linear_velocity) const -{ - return calculate_control_signals(curr_velocities, displacements, dt, - target_linear_velocity); -} - std::array SlotcarCommon::calculate_joint_control_signals( const std::array& w_tire, const std::pair& displacements, @@ -457,13 +447,31 @@ std::string to_str(uint32_t type) return "UNKNOWN: " + std::to_string(type) + "??"; } -// First value of par is x_target, second is yaw_target -std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, +SlotcarCommon::UpdateResult SlotcarCommon::update(const Eigen::Isometry3d& pose, const std::vector& obstacle_positions, const double time) { std::lock_guard lock(_mutex); - std::pair displacements; + _pose = pose; + publish_robot_state(time); + switch (this->_steering_type) + { + case SteeringType::DIFF_DRIVE: + return update_diff_drive(obstacle_positions, time); + case SteeringType::ACKERMANN: + // TODO(anyone) use obstacle_positions for emergency stop for ackermann + return update_ackermann(obstacle_positions, time); + default: + return UpdateResult(); + } +} + +// First value of par is x_target, second is yaw_target +SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive( + const std::vector& obstacle_positions, + const double time) +{ + UpdateResult result; const int32_t t_sec = static_cast(time); const uint32_t t_nsec = static_cast((time-static_cast(t_sec)) *1e9); @@ -471,9 +479,6 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, double dt = time - _last_update_time; _last_update_time = time; - _pose = pose; - publish_robot_state(time); - // Update battery state of charge if (_initialized_pose) { @@ -531,7 +536,7 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, _initialized_pose = true; if (trajectory.empty()) - return displacements; + return result; Eigen::Vector3d current_heading = compute_heading(_pose); @@ -580,7 +585,7 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, compute_heading(trajectory.at(_traj_wp_idx+1)); double dir = 1.0; - displacements.second = compute_change_in_rotation( + result.w = compute_change_in_rotation( current_heading, dpos_next, &goal_heading, &dir); if (dir < 0.0) @@ -589,7 +594,7 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, else { const auto goal_heading = compute_heading(trajectory.at(_traj_wp_idx)); - displacements.second = compute_change_in_rotation( + result.w = compute_change_in_rotation( current_heading, goal_heading); } @@ -599,7 +604,7 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, { _traj_wp_idx++; if (_remaining_path.empty()) - return displacements; + return result; _remaining_path.erase(_remaining_path.begin()); RCLCPP_INFO(logger(), @@ -621,7 +626,7 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, const double d_yaw_tolerance = 5.0 * M_PI / 180.0; auto goal_heading = compute_heading(trajectory.at(_traj_wp_idx)); double dir = 1.0; - displacements.second = + result.w = compute_change_in_rotation(current_heading, dpos, &goal_heading, &dir); if (dir < 0.0) current_heading *= -1.0; @@ -629,25 +634,18 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, // If d_yaw is less than a certain tolerance (i.e. we don't need to spin // too much), then we'll include the forward velocity. Otherwise, we will // only spin in place until we are oriented in the desired direction. - displacements.first = std::abs(displacements.second) < + result.v = std::abs(result.w) < d_yaw_tolerance ? dir * dpos_mag : 0.0; } } else { const auto goal_heading = compute_heading(trajectory.back()); - displacements.second = compute_change_in_rotation( + result.w = compute_change_in_rotation( current_heading, goal_heading); - // Put in a deadzone if yaw is small enough. This essentially locks the - // tires. COMMENTED OUT as it breaks rotations for some reason... - // if(std::abs(displacements.second) < std::max(0.1*M_PI/180.00, goal_yaw_tolerance)) - // { - // displacements.second = 0.0; - // } - - displacements.first = 0.0; + result.v = 0.0; } const bool immediate_pause = @@ -672,25 +670,24 @@ std::pair SlotcarCommon::update(const Eigen::Isometry3d& pose, if (stop) { // Allow spinning but not translating - displacements.first = 0.0; + result.v = 0.0; } - _rot_dir = displacements.second >= 0 ? 1 : -1; - return displacements; + _rot_dir = result.w >= 0 ? 1 : -1; + return result; } -std::pair SlotcarCommon::update_nonholonomic( - Eigen::Isometry3d& pose, double& target_linear_velocity) +SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann( + const std::vector& /*obstacle_positions*/, + const double /*time*/) { - std::lock_guard lock(_ackmann_path_req_mutex); - std::pair displacements; - target_linear_velocity = 0.0; - if (_nonholonomic_traj_idx >= nonholonomic_trajectory.size()) - return displacements; + UpdateResult result; + if (_ackermann_traj_idx >= ackermann_trajectory.size()) + return result; - const NonHolonomicTrajectory& traj = - nonholonomic_trajectory[_nonholonomic_traj_idx]; + const AckermannTrajectory& traj = + ackermann_trajectory[_ackermann_traj_idx]; double dpos_mag = std::numeric_limits::max(); double wp_range = 0.75; bool close_enough = false; @@ -698,17 +695,17 @@ std::pair SlotcarCommon::update_nonholonomic( if (traj.turning == false) { Eigen::Vector3d to_waypoint = Eigen::Vector3d(traj.x1.x(), traj.x1.y(), 0) - - pose.translation(); + _pose.translation(); to_waypoint(2) = 0.0; const Eigen::Vector3d dpos = to_waypoint; dpos_mag = dpos.norm(); - displacements.first = dpos_mag >= wp_range ? dpos_mag : 0.0; + result.v = dpos_mag >= wp_range ? dpos_mag : 0.0; // figure out where we are relative to the goal point - Eigen::Vector2d position(pose.translation().x(), pose.translation().y()); + Eigen::Vector2d position(_pose.translation().x(), _pose.translation().y()); Eigen::Vector2d dest_pt = traj.x1; Eigen::Vector2d forward = traj.v1; Eigen::Vector2d dest_pt_to_current_position = position - dest_pt; @@ -717,38 +714,35 @@ std::pair SlotcarCommon::update_nonholonomic( // we behind the goal point, turn to suit our needs if (dotp_location < 0.0) { - Eigen::Vector2d heading = pose.linear().block<2, 1>(0, 0); + Eigen::Vector2d heading = _pose.linear().block<2, 1>(0, 0); heading = heading.normalized(); Eigen::Vector2d dpos_norm(dpos.x(), dpos.y()); dpos_norm = dpos_norm.normalized(); double dotp = heading.dot(dpos_norm); double cross = heading.x() * dpos_norm.y() - heading.y() * dpos_norm.x(); - displacements.second = cross < 0.0 ? -acos(dotp) : acos(dotp); + result.w = cross < 0.0 ? -acos(dotp) : acos(dotp); } else - displacements.second = 0.0; - - // printf("_nonholonomic_traj_idx %d dpos_mag: %g displacements %g, %g\n", - // _nonholonomic_traj_idx, dpos_mag, displacements.first, displacements.second); + result.w = 0.0; close_enough = (dpos_mag < wp_range) || dotp_location >= 0.0; - if (_nonholonomic_traj_idx != (nonholonomic_trajectory.size() - 1)) - target_linear_velocity = _nominal_drive_speed; + if (_ackermann_traj_idx != (ackermann_trajectory.size() - 1)) + result.speed = _nominal_drive_speed; } else { - Eigen::Vector2d position(pose.translation().x(), pose.translation().y()); - target_linear_velocity = _nominal_drive_speed; + Eigen::Vector2d position(_pose.translation().x(), _pose.translation().y()); + result.speed = _nominal_drive_speed; - Eigen::Vector2d heading = pose.linear().block<2, 1>(0, 0); + Eigen::Vector2d heading = _pose.linear().block<2, 1>(0, 0); heading = heading.normalized(); Eigen::Vector2d target_heading = traj.v1; double heading_dotp = heading.dot(target_heading); double cross = heading.x() * target_heading.y() - heading.y() * target_heading.x(); - displacements.second = cross < + result.w = cross < 0.0 ? -acos(heading_dotp) : acos(heading_dotp); // figure out if we're close enough @@ -757,7 +751,7 @@ std::pair SlotcarCommon::update_nonholonomic( Eigen::Vector2d dest_pt_to_current_position = position - dest_pt; double dotp = forward.dot(dest_pt_to_current_position); if (dotp < 0.0) - displacements.first = dest_pt_to_current_position.norm(); + result.v = dest_pt_to_current_position.norm(); dpos_mag = (Eigen::Vector2d(traj.x1.x(), traj.x1.y()) - position).norm(); @@ -765,9 +759,9 @@ std::pair SlotcarCommon::update_nonholonomic( } if (close_enough) - ++_nonholonomic_traj_idx; + ++_ackermann_traj_idx; - return displacements; + return result; } bool SlotcarCommon::emergency_stop( @@ -1046,13 +1040,3 @@ double SlotcarCommon::compute_discharge( return dSOC; } - -bool SlotcarCommon::is_ackermann_steered() const -{ - return this->_steering_type == SteeringType::ACKERMANN; -} - -rmf_robot_sim_common::SteeringType SlotcarCommon::get_steering_type() const -{ - return this->_steering_type; -} diff --git a/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp b/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp index 515f6bd..8070d24 100644 --- a/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp @@ -164,11 +164,11 @@ void SlotcarPlugin::OnUpdate() auto pose = _model->WorldPose(); auto obstacle_positions = get_obstacle_positions(world); - auto displacements = + auto update_result = dataPtr->update(rmf_plugins_utils::convert_pose(pose), obstacle_positions, time); - send_control_signals(displacements, dt); + send_control_signals({update_result.v, update_result.w}, dt); } GZ_REGISTER_MODEL_PLUGIN(SlotcarPlugin) diff --git a/rmf_robot_sim_ignition_plugins/src/slotcar.cpp b/rmf_robot_sim_ignition_plugins/src/slotcar.cpp index 8ef4f04..66528b1 100644 --- a/rmf_robot_sim_ignition_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_ignition_plugins/src/slotcar.cpp @@ -171,7 +171,7 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, double v_robot = _prev_v_command; double w_robot = _prev_w_command; std::array target_vels; - target_vels = dataPtr->calculate_model_control_signals({v_robot, w_robot}, + target_vels = dataPtr->calculate_control_signals({v_robot, w_robot}, displacements, dt, target_linear_velocity); lin_vel_cmd->Data()[0] = target_vels[0]; @@ -361,41 +361,15 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info, (std::chrono::duration_cast(info.simTime).count()) * 1e-9; - using namespace rmf_robot_sim_common; - if (dataPtr->get_steering_type() == SteeringType::ACKERMANN) - { - double target_linear_velocity = 0.0; - auto& pose = ecm.Component(_entity)->Data(); - auto isometry_pose = rmf_plugins_utils::convert_pose(pose); - auto displacements = dataPtr->update_nonholonomic(isometry_pose, - target_linear_velocity); - - //convert back to account for flips - pose = rmf_plugins_utils::convert_to_pose( - isometry_pose); - - send_control_signals(ecm, displacements, _payloads, dt, - target_linear_velocity); - } - else if (dataPtr->get_steering_type() == SteeringType::DIFF_DRIVE) - { - auto pose = ecm.Component(_entity)->Data(); - auto obstacle_positions = get_obstacle_positions(ecm); - - auto p = pose.Pos(); + auto pose = ecm.Component(_entity)->Data(); + auto obstacle_positions = get_obstacle_positions(ecm); - //printf("%s: %g %g!\n", dataPtr->model_name().c_str(), p.X(), p.Y()); + auto update_result = + dataPtr->update(rmf_plugins_utils::convert_pose(pose), + obstacle_positions, time); - auto displacements = - dataPtr->update(rmf_plugins_utils::convert_pose(pose), - obstacle_positions, time); - - send_control_signals(ecm, displacements, _payloads, dt); - } - else - { - RCLCPP_INFO(dataPtr->logger(), "Unknown steering type"); - } + send_control_signals(ecm, {update_result.v, update_result.w}, _payloads, dt, + update_result.speed); } IGNITION_ADD_PLUGIN(