From e35eefee8d57058dba601ae9157a6f8848cfa082 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 21 Apr 2022 14:48:31 +0200 Subject: [PATCH 01/27] Set up initial node for standalone path tracker --- CMakeLists.txt | 5 + src/path_tracker_node.cpp | 565 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 570 insertions(+) create mode 100644 src/path_tracker_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bb137b63..0ce5d043 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,10 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_executable(path_tracker src/path_tracker_node.cpp) +add_dependencies(path_tracker ${catkin_EXPORTED_TARGETS}) +target_link_libraries(path_tracker ${catkin_LIBRARIES}) + include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) include_directories(include) @@ -85,6 +89,7 @@ roslint_add_test() install( TARGETS ${PROJECT_NAME} + path_tracker RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp new file mode 100644 index 00000000..b3c09b0e --- /dev/null +++ b/src/path_tracker_node.cpp @@ -0,0 +1,565 @@ +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "common.hpp" +#include +#include + +// namespace path_tracking_pid +// { +// namespace +// { +// constexpr double MAP_PARALLEL_THRESH = 0.2; +// constexpr double DT_MAX = 1.5; + +// /** +// * Convert the plan from geometry message format to tf2 format. +// * +// * @param[in] plan Plan to convert. +// * @return Converted plan. +// */ +// std::vector convert_plan(const std::vector & plan) +// { +// auto result = std::vector{}; + +// result.reserve(plan.size()); +// std::transform( +// plan.cbegin(), plan.cend(), std::back_inserter(result), +// [](const geometry_msgs::PoseStamped & msg) { return tf2_convert(msg.pose); }); + +// return result; +// } + +// } // namespace + +// void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & config) +// { +// pid_controller_.configure(config); +// controller_debug_enabled_ = config.controller_debug_enabled; +// } + +// void TrackingPidLocalPlanner::initialize( +// std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) +// { +// ros::NodeHandle nh("~/" + name); +// ros::NodeHandle gn; +// ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); +// // setup dynamic reconfigure +// pid_server_ = +// std::make_unique>(config_mutex_, nh); +// pid_server_->setCallback( +// [this](auto & config, auto /*unused*/) { this->reconfigure_pid(config); }); +// pid_controller_.setEnabled(false); + +// bool holonomic_robot; +// nh.param("holonomic_robot", holonomic_robot, false); +// pid_controller_.setHolonomic(holonomic_robot); + +// bool estimate_pose_angle; +// nh.param("estimate_pose_angle", estimate_pose_angle, false); +// pid_controller_.setEstimatePoseAngle(estimate_pose_angle); + +// nh.param("base_link_frame", base_link_frame_, "base_link"); + +// nh.param("use_tricycle_model", use_tricycle_model_, false); +// nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); + +// visualization_ = std::make_unique(nh); +// debug_pub_ = nh.advertise("debug", 1); +// path_pub_ = nh.advertise("visualization_path", 1, true); + +// sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this); +// sub_vel_max_external_ = +// nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); +// feedback_pub_ = nh.advertise("feedback", 1); + +// map_frame_ = costmap->getGlobalFrameID(); +// costmap_ = costmap; +// tf_ = tf; + +// initialized_ = true; +// } + +// bool TrackingPidLocalPlanner::setPlan(const std::vector & global_plan) +// { +// if (!initialized_) { +// ROS_ERROR( +// "path_tracking_pid has not been initialized, please call initialize() before using this " +// "planner"); +// return false; +// } + +// auto global_plan_map_frame = global_plan; + +// std::string path_frame = global_plan_map_frame.at(0).header.frame_id; +// ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan_map_frame.size()); +// ROS_DEBUG("Plan is defined in frame '%s'", path_frame.c_str()); + +// /* If frame of received plan is not equal to mbf-map_frame, translate first */ +// if (map_frame_ != path_frame) { +// ROS_DEBUG( +// "Transforming plan since my global_frame = '%s' and my plan is in frame: '%s'", +// map_frame_.c_str(), path_frame.c_str()); +// geometry_msgs::TransformStamped tf_transform; +// tf_transform = tf_->lookupTransform(map_frame_, path_frame, ros::Time(0)); +// // Check alignment, when path-frame is severly mis-aligned show error +// double yaw; +// double pitch; +// double roll; +// tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll); +// if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) { +// ROS_ERROR( +// "Path is given in %s frame which is severly mis-aligned with our map-frame: %s", +// path_frame.c_str(), map_frame_.c_str()); +// } +// for (auto & pose_stamped : global_plan_map_frame) { +// tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform); +// pose_stamped.header.frame_id = map_frame_; +// // 'Project' plan by removing z-component +// pose_stamped.pose.position.z = 0.0; +// } +// } + +// if (controller_debug_enabled_) { +// nav_msgs::Path received_path; +// received_path.header = global_plan_map_frame.at(0).header; +// received_path.poses = global_plan_map_frame; +// path_pub_.publish(received_path); +// } + +// try { +// ROS_DEBUG( +// "map_frame: %s, plan_frame: %s, base_link_frame: %s", map_frame_.c_str(), path_frame.c_str(), +// base_link_frame_.c_str()); +// tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); +// } catch (const tf2::TransformException & ex) { +// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); +// return false; +// } + +// // Feasability check, but only when not resuming with odom-vel +// if ( +// pid_controller_.getConfig().init_vel_method != Pid_Odom && +// pid_controller_.getConfig().init_vel_max_diff >= 0.0 && +// std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getCurrentForwardVelocity()) > +// pid_controller_.getConfig().init_vel_max_diff) { +// ROS_ERROR( +// "Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", +// latest_odom_.twist.twist.linear.x, pid_controller_.getCurrentForwardVelocity()); +// return false; +// } + +// if (use_tricycle_model_) { +// try { +// ROS_DEBUG( +// "base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), +// steered_wheel_frame_.c_str()); +// tf_base_to_steered_wheel_stamped_ = +// tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); +// } catch (const tf2::TransformException & ex) { +// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); +// ROS_ERROR( +// "Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will " +// "be disabled"); +// use_tricycle_model_ = false; +// } + +// pid_controller_.setTricycleModel( +// use_tricycle_model_, +// tf2_convert(tf_base_to_steered_wheel_stamped_.transform)); + +// // TODO(clopez): subscribe to steered wheel odom +// geometry_msgs::Twist steering_odom_twist; +// if (!pid_controller_.setPlan( +// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, +// tf2_convert(tf_base_to_steered_wheel_stamped_.transform), +// steering_odom_twist, convert_plan(global_plan_map_frame))) { +// return false; +// } +// } else { +// if (!pid_controller_.setPlan( +// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, +// convert_plan(global_plan_map_frame))) { +// return false; +// } +// } + +// pid_controller_.setEnabled(true); +// active_goal_ = true; +// prev_time_ = ros::Time(0); +// return true; +// } + +// std::optional TrackingPidLocalPlanner::computeVelocityCommands() +// { +// ros::Time now = ros::Time::now(); +// if (prev_time_.isZero()) { +// prev_time_ = now - prev_dt_; // Initialisation round +// } +// ros::Duration dt = now - prev_time_; +// if (dt.isZero()) { +// ROS_ERROR_THROTTLE( +// 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); +// auto cmd_vel = geometry_msgs::Twist(); +// cmd_vel.linear.x = pid_controller_.getCurrentForwardVelocity(); +// cmd_vel.angular.z = pid_controller_.getCurrentYawVelocity(); +// // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return +// // false because of https://github.com/magazino/move_base_flex/issues/195 so the current +// // velocity is send instead. +// return cmd_vel; +// } +// if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { +// ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); +// return std::nullopt; +// } +// try { +// ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); +// tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); +// } catch (const tf2::TransformException & ex) { +// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); +// active_goal_ = false; +// return std::nullopt; +// } + +// // Handle obstacles +// if (pid_controller_.getConfig().anti_collision) { +// const std::vector footprint = costmap_->getRobotFootprint(); +// auto cost = projectedCollisionCost( +// costmap_->getCostmap(), footprint, projectionSteps(), visualization_, map_frame_); + +// if (cost >= costmap_2d::LETHAL_OBSTACLE) { +// pid_controller_.setVelMaxObstacle(0.0); +// } else if (pid_controller_.getConfig().obstacle_speed_reduction) { +// double max_vel = pid_controller_.getConfig().max_x_vel; +// double reduction_factor = static_cast(cost) / costmap_2d::LETHAL_OBSTACLE; +// double limit = max_vel * (1 - reduction_factor); +// ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); +// pid_controller_.setVelMaxObstacle(limit); +// } else { +// pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf +// } +// } else { +// pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf +// } + +// const auto update_result = pid_controller_.update_with_limits( +// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); + +// path_tracking_pid::PidFeedback feedback_msg; +// feedback_msg.eda = ros::Duration(update_result.eda); +// feedback_msg.progress = update_result.progress; +// feedback_pub_.publish(feedback_msg); + +// if (cancel_requested_) { +// path_tracking_pid::PidConfig config = pid_controller_.getConfig(); +// // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence +// // the sign propagates correctly +// config.target_x_vel = std::copysign(0.0, config.target_x_vel); +// config.target_end_x_vel = std::copysign(0.0, config.target_x_vel); +// boost::recursive_mutex::scoped_lock lock(config_mutex_); +// // When updating from own server no callback is called. Thus controller is updated first and then server is notified +// pid_controller_.configure(config); +// pid_server_->updateConfig(config); +// lock.unlock(); +// cancel_requested_ = false; +// } + +// if (controller_debug_enabled_) { +// debug_pub_.publish(update_result.pid_debug); + +// // publish rviz visualization +// std_msgs::Header header; +// header.stamp = now; +// header.frame_id = map_frame_; +// const auto tfCurPose = tf2_convert(tfCurPoseStamped_.transform); +// visualization_->publishAxlePoint(header, tfCurPose); +// visualization_->publishControlPoint(header, pid_controller_.getCurrentWithCarrot()); +// visualization_->publishGoalPoint(header, pid_controller_.getCurrentGoal()); +// visualization_->publishPlanPoint(header, pid_controller_.getCurrentPosOnPlan()); +// } + +// prev_time_ = now; +// prev_dt_ = +// dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) +// return update_result.velocity_command; +// } + +// std::vector TrackingPidLocalPlanner::projectionSteps() +// { +// // Check how far we should check forward +// double x_vel = pid_controller_.getCurrentForwardVelocity(); +// double collision_look_ahead_distance = +// x_vel * x_vel / (2 * pid_controller_.getConfig().target_x_decc) + +// pid_controller_.getConfig().collision_look_ahead_length_offset; +// uint n_steps = std::ceil( +// collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); +// double x_resolution = collision_look_ahead_distance / std::max(static_cast(n_steps), 1); + +// // Define a x_step transform which will be used to step forward the position. +// tf2::Transform x_step_tf; +// double target_x_vel = pid_controller_.getConfig().target_x_vel; +// double max_abs_x_vel = std::abs(x_vel) > std::abs(target_x_vel) ? x_vel : target_x_vel; +// x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); + +// // Keep track of the projected position on the path. +// auto projected_global_plan_index = pid_controller_.getCurrentGlobalPlanIndex(); + +// // Step until lookahead is reached, for every step project the pose back to the path +// std::vector step_points; +// std::vector poses_on_path_points; +// std::vector projected_steps_tf; +// auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); +// projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link +// projected_step_tf = +// pid_controller_.findPoseOnPlan(projected_step_tf, projected_global_plan_index).pose; +// projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose +// for (uint step = 0; step < n_steps; step++) { +// tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; +// projected_step_tf = +// pid_controller_.findPoseOnPlan(next_straight_step_tf, projected_global_plan_index).pose; +// projected_steps_tf.push_back(projected_step_tf); + +// // Fill markers: +// step_points.push_back(next_straight_step_tf.getOrigin()); +// poses_on_path_points.push_back(projected_step_tf.getOrigin()); +// } + +// // Visualize +// std_msgs::Header header; +// header.stamp = ros::Time::now(); +// header.frame_id = map_frame_; +// visualization_->publishExtrapolatedPoses(header, step_points); +// visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); + +// return projected_steps_tf; +// } + +// boost::geometry::model::ring TrackingPidLocalPlanner::projectionFootprint( +// const std::vector & footprint, +// const std::vector & projected_steps, std::unique_ptr & viz, +// const std::string viz_frame) +// { +// std::vector projected_footprint_points; +// polygon_t previous_footprint_xy; +// polygon_t projected_polygon; +// for (const auto & projection_tf : projected_steps) { +// // Project footprint forward +// double x = projection_tf.getOrigin().x(); +// double y = projection_tf.getOrigin().y(); +// double yaw = tf2::getYaw(projection_tf.getRotation()); + +// // Project footprint forward +// std::vector footprint_proj; +// costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj); + +// // Append footprint to polygon +// polygon_t two_footprints = previous_footprint_xy; +// previous_footprint_xy.clear(); +// for (const auto & point : footprint_proj) { +// boost::geometry::append(two_footprints, point); +// boost::geometry::append(previous_footprint_xy, point); +// } + +// boost::geometry::correct(two_footprints); +// polygon_t two_footprint_hull; +// boost::geometry::convex_hull(two_footprints, two_footprint_hull); +// projected_polygon = union_(projected_polygon, two_footprint_hull); + +// // Add footprint to marker +// geometry_msgs::Point previous_point = footprint_proj.back(); +// for (const auto & point : footprint_proj) { +// projected_footprint_points.push_back(tf2_convert(previous_point)); +// projected_footprint_points.push_back(tf2_convert(point)); +// previous_point = point; +// } +// } + +// std_msgs::Header header; +// header.stamp = ros::Time::now(); +// header.frame_id = viz_frame; +// viz->publishCollisionFootprint(header, projected_footprint_points); + +// return projected_polygon; +// } + +// uint8_t TrackingPidLocalPlanner::projectedCollisionCost( +// costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, +// const std::vector & projected_steps, std::unique_ptr & viz, +// const std::string viz_frame) +// { +// auto collision_polygon = projectionFootprint(footprint, projected_steps, viz, viz_frame); + +// // Calculate cost by checking base link location in costmap +// uint8_t max_projected_step_cost = 0; +// for (const auto & projection_tf : projected_steps) { +// int map_x, map_y; +// costmap2d->worldToMapEnforceBounds( +// projection_tf.getOrigin().x(), projection_tf.getOrigin().y(), map_x, map_y); +// uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); +// if (projected_step_cost > max_projected_step_cost) { +// max_projected_step_cost = projected_step_cost; +// } +// } + +// // Create a convex hull so we can use costmap2d->convexFillCells +// polygon_t collision_polygon_hull; +// boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); +// std::vector collision_polygon_hull_map; + +// // Convert to map coordinates +// for (const auto & point : collision_polygon_hull) { +// int map_x; +// int map_y; +// costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); +// costmap_2d::MapLocation map_point{static_cast(map_x), static_cast(map_y)}; +// collision_polygon_hull_map.push_back(map_point); +// } + +// // Get the relevant cells +// std::vector cells_in_polygon; +// costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); + +// // Get the max cost inside the concave polygon +// tf2::Vector3 collision_point; +// uint8_t max_cost = 0.0; +// for (const auto & cell_in_polygon : cells_in_polygon) { +// // Cost checker is cheaper than polygon checker, so lets do that first +// uint8_t cell_cost = costmap2d->getCost(cell_in_polygon.x, cell_in_polygon.y); +// if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) { +// // Check if in concave polygon +// geometry_msgs::Point point; +// costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); +// if (boost::geometry::within(point, collision_polygon)) { +// max_cost = cell_cost; +// // Set collision indicator on suspected cell with current cost +// collision_point = tf2_convert(point); +// if (max_cost >= costmap_2d::LETHAL_OBSTACLE) { +// max_projected_step_cost = max_cost; +// break; // Collision detected, no need to evaluate further +// } +// } +// } +// } + +// // Fiddle the polygon into a marker message +// std::vector collision_hull_points; +// for (const geometry_msgs::Point point : collision_polygon) { +// tf2::Vector3 point_tf2; +// tf2::fromMsg(point, point_tf2); +// collision_hull_points.push_back(point_tf2); +// } +// std_msgs::Header header; +// header.stamp = ros::Time::now(); +// header.frame_id = viz_frame; +// viz->publishCollisionObject(header, max_cost, collision_point); +// viz->publishCollisionPolygon(header, collision_hull_points); + +// return max_projected_step_cost; +// } + +// uint32_t TrackingPidLocalPlanner::computeVelocityCommands( +// const geometry_msgs::PoseStamped & /* pose */, const geometry_msgs::TwistStamped & /* velocity */, +// geometry_msgs::TwistStamped & cmd_vel, std::string & /* message */) +// { +// if (!initialized_) { +// ROS_ERROR( +// "path_tracking_pid has not been initialized, please call initialize() before using this " +// "planner"); +// active_goal_ = false; +// return mbf_msgs::ExePathResult::NOT_INITIALIZED; +// } +// // TODO(Cesar): Use provided pose and odom +// const auto opt_cmd_vel = computeVelocityCommands(); +// if (!opt_cmd_vel) { +// active_goal_ = false; +// return mbf_msgs::ExePathResult::FAILURE; +// } +// cmd_vel.twist = *opt_cmd_vel; +// cmd_vel.header.stamp = ros::Time::now(); +// cmd_vel.header.frame_id = base_link_frame_; + +// bool moving = std::abs(cmd_vel.twist.linear.x) > VELOCITY_EPS; +// if (cancel_in_progress_) { +// if (!moving) { +// ROS_INFO( +// "Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); +// cancel_in_progress_ = false; +// active_goal_ = false; +// return mbf_msgs::ExePathResult::CANCELED; +// } +// ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); +// return to_underlying(ComputeVelocityCommandsResult::GRACEFULLY_CANCELLING); +// } + +// if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) { +// active_goal_ = false; +// return mbf_msgs::ExePathResult::BLOCKED_PATH; +// } + +// if (isGoalReached()) { +// active_goal_ = false; +// } +// return mbf_msgs::ExePathResult::SUCCESS; +// } + +// bool TrackingPidLocalPlanner::isGoalReached() const +// { +// // Return reached boolean, but never succeed when we're preempting +// return pid_controller_.isEndReached() && !cancel_in_progress_; +// } + +// bool TrackingPidLocalPlanner::isGoalReached( +// double /* dist_tolerance */, double /* angle_tolerance */) +// { +// return isGoalReached(); +// } + +// bool TrackingPidLocalPlanner::cancel() +// { +// // This function runs in a separate thread +// cancel_requested_ = true; +// cancel_in_progress_ = true; +// ros::Rate r(10); +// ROS_INFO("Cancel requested, waiting in loop for cancel to finish"); +// while (active_goal_) { +// r.sleep(); +// } +// ROS_INFO("Finished waiting loop, done cancelling"); +// return true; +// } + +// void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry & odom_msg) +// { +// latest_odom_ = odom_msg; +// } + +// void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64 & msg) +// { +// pid_controller_.setVelMaxExternal(msg.data); +// } + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "path_tracker"); + ROS_INFO("Hello Path Tracker"); + + ros::spin(); + + ROS_INFO("Bye-bye Path Tracker"); + + return 0; +} + +// } // namespace path_tracking_pid From 3e8c5a581e44f29e11b629056dc536692544c841 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 21 Apr 2022 16:05:24 +0200 Subject: [PATCH 02/27] Also depend on own library of course --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ce5d043..c8d1e0aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,8 +73,8 @@ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) add_executable(path_tracker src/path_tracker_node.cpp) -add_dependencies(path_tracker ${catkin_EXPORTED_TARGETS}) -target_link_libraries(path_tracker ${catkin_LIBRARIES}) +add_dependencies(path_tracker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(path_tracker ${PROJECT_NAME} ${catkin_LIBRARIES}) include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) include_directories(include) From 5c38c53e9c9c89ce6f90c6e87b174e613e8a653c Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 21 Apr 2022 16:05:45 +0200 Subject: [PATCH 03/27] Make node wrap the local planner plugin --- src/path_tracker_node.cpp | 569 ++------------------------------------ 1 file changed, 17 insertions(+), 552 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index b3c09b0e..d5efbc90 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -1,565 +1,30 @@ -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "common.hpp" +#include #include #include - -// namespace path_tracking_pid -// { -// namespace -// { -// constexpr double MAP_PARALLEL_THRESH = 0.2; -// constexpr double DT_MAX = 1.5; - -// /** -// * Convert the plan from geometry message format to tf2 format. -// * -// * @param[in] plan Plan to convert. -// * @return Converted plan. -// */ -// std::vector convert_plan(const std::vector & plan) -// { -// auto result = std::vector{}; - -// result.reserve(plan.size()); -// std::transform( -// plan.cbegin(), plan.cend(), std::back_inserter(result), -// [](const geometry_msgs::PoseStamped & msg) { return tf2_convert(msg.pose); }); - -// return result; -// } - -// } // namespace - -// void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & config) -// { -// pid_controller_.configure(config); -// controller_debug_enabled_ = config.controller_debug_enabled; -// } - -// void TrackingPidLocalPlanner::initialize( -// std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) -// { -// ros::NodeHandle nh("~/" + name); -// ros::NodeHandle gn; -// ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); -// // setup dynamic reconfigure -// pid_server_ = -// std::make_unique>(config_mutex_, nh); -// pid_server_->setCallback( -// [this](auto & config, auto /*unused*/) { this->reconfigure_pid(config); }); -// pid_controller_.setEnabled(false); - -// bool holonomic_robot; -// nh.param("holonomic_robot", holonomic_robot, false); -// pid_controller_.setHolonomic(holonomic_robot); - -// bool estimate_pose_angle; -// nh.param("estimate_pose_angle", estimate_pose_angle, false); -// pid_controller_.setEstimatePoseAngle(estimate_pose_angle); - -// nh.param("base_link_frame", base_link_frame_, "base_link"); - -// nh.param("use_tricycle_model", use_tricycle_model_, false); -// nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); - -// visualization_ = std::make_unique(nh); -// debug_pub_ = nh.advertise("debug", 1); -// path_pub_ = nh.advertise("visualization_path", 1, true); - -// sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this); -// sub_vel_max_external_ = -// nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); -// feedback_pub_ = nh.advertise("feedback", 1); - -// map_frame_ = costmap->getGlobalFrameID(); -// costmap_ = costmap; -// tf_ = tf; - -// initialized_ = true; -// } - -// bool TrackingPidLocalPlanner::setPlan(const std::vector & global_plan) -// { -// if (!initialized_) { -// ROS_ERROR( -// "path_tracking_pid has not been initialized, please call initialize() before using this " -// "planner"); -// return false; -// } - -// auto global_plan_map_frame = global_plan; - -// std::string path_frame = global_plan_map_frame.at(0).header.frame_id; -// ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan_map_frame.size()); -// ROS_DEBUG("Plan is defined in frame '%s'", path_frame.c_str()); - -// /* If frame of received plan is not equal to mbf-map_frame, translate first */ -// if (map_frame_ != path_frame) { -// ROS_DEBUG( -// "Transforming plan since my global_frame = '%s' and my plan is in frame: '%s'", -// map_frame_.c_str(), path_frame.c_str()); -// geometry_msgs::TransformStamped tf_transform; -// tf_transform = tf_->lookupTransform(map_frame_, path_frame, ros::Time(0)); -// // Check alignment, when path-frame is severly mis-aligned show error -// double yaw; -// double pitch; -// double roll; -// tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll); -// if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) { -// ROS_ERROR( -// "Path is given in %s frame which is severly mis-aligned with our map-frame: %s", -// path_frame.c_str(), map_frame_.c_str()); -// } -// for (auto & pose_stamped : global_plan_map_frame) { -// tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform); -// pose_stamped.header.frame_id = map_frame_; -// // 'Project' plan by removing z-component -// pose_stamped.pose.position.z = 0.0; -// } -// } - -// if (controller_debug_enabled_) { -// nav_msgs::Path received_path; -// received_path.header = global_plan_map_frame.at(0).header; -// received_path.poses = global_plan_map_frame; -// path_pub_.publish(received_path); -// } - -// try { -// ROS_DEBUG( -// "map_frame: %s, plan_frame: %s, base_link_frame: %s", map_frame_.c_str(), path_frame.c_str(), -// base_link_frame_.c_str()); -// tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); -// } catch (const tf2::TransformException & ex) { -// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); -// return false; -// } - -// // Feasability check, but only when not resuming with odom-vel -// if ( -// pid_controller_.getConfig().init_vel_method != Pid_Odom && -// pid_controller_.getConfig().init_vel_max_diff >= 0.0 && -// std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getCurrentForwardVelocity()) > -// pid_controller_.getConfig().init_vel_max_diff) { -// ROS_ERROR( -// "Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", -// latest_odom_.twist.twist.linear.x, pid_controller_.getCurrentForwardVelocity()); -// return false; -// } - -// if (use_tricycle_model_) { -// try { -// ROS_DEBUG( -// "base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), -// steered_wheel_frame_.c_str()); -// tf_base_to_steered_wheel_stamped_ = -// tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); -// } catch (const tf2::TransformException & ex) { -// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); -// ROS_ERROR( -// "Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will " -// "be disabled"); -// use_tricycle_model_ = false; -// } - -// pid_controller_.setTricycleModel( -// use_tricycle_model_, -// tf2_convert(tf_base_to_steered_wheel_stamped_.transform)); - -// // TODO(clopez): subscribe to steered wheel odom -// geometry_msgs::Twist steering_odom_twist; -// if (!pid_controller_.setPlan( -// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, -// tf2_convert(tf_base_to_steered_wheel_stamped_.transform), -// steering_odom_twist, convert_plan(global_plan_map_frame))) { -// return false; -// } -// } else { -// if (!pid_controller_.setPlan( -// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, -// convert_plan(global_plan_map_frame))) { -// return false; -// } -// } - -// pid_controller_.setEnabled(true); -// active_goal_ = true; -// prev_time_ = ros::Time(0); -// return true; -// } - -// std::optional TrackingPidLocalPlanner::computeVelocityCommands() -// { -// ros::Time now = ros::Time::now(); -// if (prev_time_.isZero()) { -// prev_time_ = now - prev_dt_; // Initialisation round -// } -// ros::Duration dt = now - prev_time_; -// if (dt.isZero()) { -// ROS_ERROR_THROTTLE( -// 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); -// auto cmd_vel = geometry_msgs::Twist(); -// cmd_vel.linear.x = pid_controller_.getCurrentForwardVelocity(); -// cmd_vel.angular.z = pid_controller_.getCurrentYawVelocity(); -// // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return -// // false because of https://github.com/magazino/move_base_flex/issues/195 so the current -// // velocity is send instead. -// return cmd_vel; -// } -// if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { -// ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); -// return std::nullopt; -// } -// try { -// ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); -// tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); -// } catch (const tf2::TransformException & ex) { -// ROS_ERROR("Received an exception trying to transform: %s", ex.what()); -// active_goal_ = false; -// return std::nullopt; -// } - -// // Handle obstacles -// if (pid_controller_.getConfig().anti_collision) { -// const std::vector footprint = costmap_->getRobotFootprint(); -// auto cost = projectedCollisionCost( -// costmap_->getCostmap(), footprint, projectionSteps(), visualization_, map_frame_); - -// if (cost >= costmap_2d::LETHAL_OBSTACLE) { -// pid_controller_.setVelMaxObstacle(0.0); -// } else if (pid_controller_.getConfig().obstacle_speed_reduction) { -// double max_vel = pid_controller_.getConfig().max_x_vel; -// double reduction_factor = static_cast(cost) / costmap_2d::LETHAL_OBSTACLE; -// double limit = max_vel * (1 - reduction_factor); -// ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); -// pid_controller_.setVelMaxObstacle(limit); -// } else { -// pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf -// } -// } else { -// pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf -// } - -// const auto update_result = pid_controller_.update_with_limits( -// tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); - -// path_tracking_pid::PidFeedback feedback_msg; -// feedback_msg.eda = ros::Duration(update_result.eda); -// feedback_msg.progress = update_result.progress; -// feedback_pub_.publish(feedback_msg); - -// if (cancel_requested_) { -// path_tracking_pid::PidConfig config = pid_controller_.getConfig(); -// // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence -// // the sign propagates correctly -// config.target_x_vel = std::copysign(0.0, config.target_x_vel); -// config.target_end_x_vel = std::copysign(0.0, config.target_x_vel); -// boost::recursive_mutex::scoped_lock lock(config_mutex_); -// // When updating from own server no callback is called. Thus controller is updated first and then server is notified -// pid_controller_.configure(config); -// pid_server_->updateConfig(config); -// lock.unlock(); -// cancel_requested_ = false; -// } - -// if (controller_debug_enabled_) { -// debug_pub_.publish(update_result.pid_debug); - -// // publish rviz visualization -// std_msgs::Header header; -// header.stamp = now; -// header.frame_id = map_frame_; -// const auto tfCurPose = tf2_convert(tfCurPoseStamped_.transform); -// visualization_->publishAxlePoint(header, tfCurPose); -// visualization_->publishControlPoint(header, pid_controller_.getCurrentWithCarrot()); -// visualization_->publishGoalPoint(header, pid_controller_.getCurrentGoal()); -// visualization_->publishPlanPoint(header, pid_controller_.getCurrentPosOnPlan()); -// } - -// prev_time_ = now; -// prev_dt_ = -// dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) -// return update_result.velocity_command; -// } - -// std::vector TrackingPidLocalPlanner::projectionSteps() -// { -// // Check how far we should check forward -// double x_vel = pid_controller_.getCurrentForwardVelocity(); -// double collision_look_ahead_distance = -// x_vel * x_vel / (2 * pid_controller_.getConfig().target_x_decc) + -// pid_controller_.getConfig().collision_look_ahead_length_offset; -// uint n_steps = std::ceil( -// collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); -// double x_resolution = collision_look_ahead_distance / std::max(static_cast(n_steps), 1); - -// // Define a x_step transform which will be used to step forward the position. -// tf2::Transform x_step_tf; -// double target_x_vel = pid_controller_.getConfig().target_x_vel; -// double max_abs_x_vel = std::abs(x_vel) > std::abs(target_x_vel) ? x_vel : target_x_vel; -// x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); - -// // Keep track of the projected position on the path. -// auto projected_global_plan_index = pid_controller_.getCurrentGlobalPlanIndex(); - -// // Step until lookahead is reached, for every step project the pose back to the path -// std::vector step_points; -// std::vector poses_on_path_points; -// std::vector projected_steps_tf; -// auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); -// projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link -// projected_step_tf = -// pid_controller_.findPoseOnPlan(projected_step_tf, projected_global_plan_index).pose; -// projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose -// for (uint step = 0; step < n_steps; step++) { -// tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; -// projected_step_tf = -// pid_controller_.findPoseOnPlan(next_straight_step_tf, projected_global_plan_index).pose; -// projected_steps_tf.push_back(projected_step_tf); - -// // Fill markers: -// step_points.push_back(next_straight_step_tf.getOrigin()); -// poses_on_path_points.push_back(projected_step_tf.getOrigin()); -// } - -// // Visualize -// std_msgs::Header header; -// header.stamp = ros::Time::now(); -// header.frame_id = map_frame_; -// visualization_->publishExtrapolatedPoses(header, step_points); -// visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); - -// return projected_steps_tf; -// } - -// boost::geometry::model::ring TrackingPidLocalPlanner::projectionFootprint( -// const std::vector & footprint, -// const std::vector & projected_steps, std::unique_ptr & viz, -// const std::string viz_frame) -// { -// std::vector projected_footprint_points; -// polygon_t previous_footprint_xy; -// polygon_t projected_polygon; -// for (const auto & projection_tf : projected_steps) { -// // Project footprint forward -// double x = projection_tf.getOrigin().x(); -// double y = projection_tf.getOrigin().y(); -// double yaw = tf2::getYaw(projection_tf.getRotation()); - -// // Project footprint forward -// std::vector footprint_proj; -// costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj); - -// // Append footprint to polygon -// polygon_t two_footprints = previous_footprint_xy; -// previous_footprint_xy.clear(); -// for (const auto & point : footprint_proj) { -// boost::geometry::append(two_footprints, point); -// boost::geometry::append(previous_footprint_xy, point); -// } - -// boost::geometry::correct(two_footprints); -// polygon_t two_footprint_hull; -// boost::geometry::convex_hull(two_footprints, two_footprint_hull); -// projected_polygon = union_(projected_polygon, two_footprint_hull); - -// // Add footprint to marker -// geometry_msgs::Point previous_point = footprint_proj.back(); -// for (const auto & point : footprint_proj) { -// projected_footprint_points.push_back(tf2_convert(previous_point)); -// projected_footprint_points.push_back(tf2_convert(point)); -// previous_point = point; -// } -// } - -// std_msgs::Header header; -// header.stamp = ros::Time::now(); -// header.frame_id = viz_frame; -// viz->publishCollisionFootprint(header, projected_footprint_points); - -// return projected_polygon; -// } - -// uint8_t TrackingPidLocalPlanner::projectedCollisionCost( -// costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, -// const std::vector & projected_steps, std::unique_ptr & viz, -// const std::string viz_frame) -// { -// auto collision_polygon = projectionFootprint(footprint, projected_steps, viz, viz_frame); - -// // Calculate cost by checking base link location in costmap -// uint8_t max_projected_step_cost = 0; -// for (const auto & projection_tf : projected_steps) { -// int map_x, map_y; -// costmap2d->worldToMapEnforceBounds( -// projection_tf.getOrigin().x(), projection_tf.getOrigin().y(), map_x, map_y); -// uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); -// if (projected_step_cost > max_projected_step_cost) { -// max_projected_step_cost = projected_step_cost; -// } -// } - -// // Create a convex hull so we can use costmap2d->convexFillCells -// polygon_t collision_polygon_hull; -// boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); -// std::vector collision_polygon_hull_map; - -// // Convert to map coordinates -// for (const auto & point : collision_polygon_hull) { -// int map_x; -// int map_y; -// costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); -// costmap_2d::MapLocation map_point{static_cast(map_x), static_cast(map_y)}; -// collision_polygon_hull_map.push_back(map_point); -// } - -// // Get the relevant cells -// std::vector cells_in_polygon; -// costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); - -// // Get the max cost inside the concave polygon -// tf2::Vector3 collision_point; -// uint8_t max_cost = 0.0; -// for (const auto & cell_in_polygon : cells_in_polygon) { -// // Cost checker is cheaper than polygon checker, so lets do that first -// uint8_t cell_cost = costmap2d->getCost(cell_in_polygon.x, cell_in_polygon.y); -// if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) { -// // Check if in concave polygon -// geometry_msgs::Point point; -// costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); -// if (boost::geometry::within(point, collision_polygon)) { -// max_cost = cell_cost; -// // Set collision indicator on suspected cell with current cost -// collision_point = tf2_convert(point); -// if (max_cost >= costmap_2d::LETHAL_OBSTACLE) { -// max_projected_step_cost = max_cost; -// break; // Collision detected, no need to evaluate further -// } -// } -// } -// } - -// // Fiddle the polygon into a marker message -// std::vector collision_hull_points; -// for (const geometry_msgs::Point point : collision_polygon) { -// tf2::Vector3 point_tf2; -// tf2::fromMsg(point, point_tf2); -// collision_hull_points.push_back(point_tf2); -// } -// std_msgs::Header header; -// header.stamp = ros::Time::now(); -// header.frame_id = viz_frame; -// viz->publishCollisionObject(header, max_cost, collision_point); -// viz->publishCollisionPolygon(header, collision_hull_points); - -// return max_projected_step_cost; -// } - -// uint32_t TrackingPidLocalPlanner::computeVelocityCommands( -// const geometry_msgs::PoseStamped & /* pose */, const geometry_msgs::TwistStamped & /* velocity */, -// geometry_msgs::TwistStamped & cmd_vel, std::string & /* message */) -// { -// if (!initialized_) { -// ROS_ERROR( -// "path_tracking_pid has not been initialized, please call initialize() before using this " -// "planner"); -// active_goal_ = false; -// return mbf_msgs::ExePathResult::NOT_INITIALIZED; -// } -// // TODO(Cesar): Use provided pose and odom -// const auto opt_cmd_vel = computeVelocityCommands(); -// if (!opt_cmd_vel) { -// active_goal_ = false; -// return mbf_msgs::ExePathResult::FAILURE; -// } -// cmd_vel.twist = *opt_cmd_vel; -// cmd_vel.header.stamp = ros::Time::now(); -// cmd_vel.header.frame_id = base_link_frame_; - -// bool moving = std::abs(cmd_vel.twist.linear.x) > VELOCITY_EPS; -// if (cancel_in_progress_) { -// if (!moving) { -// ROS_INFO( -// "Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); -// cancel_in_progress_ = false; -// active_goal_ = false; -// return mbf_msgs::ExePathResult::CANCELED; -// } -// ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); -// return to_underlying(ComputeVelocityCommandsResult::GRACEFULLY_CANCELLING); -// } - -// if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) { -// active_goal_ = false; -// return mbf_msgs::ExePathResult::BLOCKED_PATH; -// } - -// if (isGoalReached()) { -// active_goal_ = false; -// } -// return mbf_msgs::ExePathResult::SUCCESS; -// } - -// bool TrackingPidLocalPlanner::isGoalReached() const -// { -// // Return reached boolean, but never succeed when we're preempting -// return pid_controller_.isEndReached() && !cancel_in_progress_; -// } - -// bool TrackingPidLocalPlanner::isGoalReached( -// double /* dist_tolerance */, double /* angle_tolerance */) -// { -// return isGoalReached(); -// } - -// bool TrackingPidLocalPlanner::cancel() -// { -// // This function runs in a separate thread -// cancel_requested_ = true; -// cancel_in_progress_ = true; -// ros::Rate r(10); -// ROS_INFO("Cancel requested, waiting in loop for cancel to finish"); -// while (active_goal_) { -// r.sleep(); -// } -// ROS_INFO("Finished waiting loop, done cancelling"); -// return true; -// } - -// void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry & odom_msg) -// { -// latest_odom_ = odom_msg; -// } - -// void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64 & msg) -// { -// pid_controller_.setVelMaxExternal(msg.data); -// } +#include +#include int main(int argc, char *argv[]) { ros::init(argc, argv, "path_tracker"); ROS_INFO("Hello Path Tracker"); + auto tplp = new path_tracking_pid::TrackingPidLocalPlanner(); + + // Call initialize with empty costmap that we don't update + // Subscribe to Paths, convert those to vector of PoseStamped for setPlan + // Call computeVelocities at some configurable rate? + + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener tfListener(tfBuffer); + + std::vector plan = std::vector(); + tplp->setPlan(plan); + // tplp->cancel(); + ros::spin(); ROS_INFO("Bye-bye Path Tracker"); return 0; -} - -// } // namespace path_tracking_pid +} \ No newline at end of file From a37f420efe98c83fb73c466b4b694d501294e40a Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 21 Apr 2022 16:37:07 +0200 Subject: [PATCH 04/27] Correctly initialized the TrackingPidLocalPlanner, busy setting up subscribers and other supporting stuff --- src/path_tracker_node.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index d5efbc90..46e4360c 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -3,10 +3,13 @@ #include #include #include +#include + int main(int argc, char *argv[]) { ros::init(argc, argv, "path_tracker"); + ros::NodeHandle nh; ROS_INFO("Hello Path Tracker"); auto tplp = new path_tracking_pid::TrackingPidLocalPlanner(); @@ -18,10 +21,21 @@ int main(int argc, char *argv[]) tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); - std::vector plan = std::vector(); - tplp->setPlan(plan); + costmap_2d::Costmap2DROS costmap("empty_costmap", tfBuffer); + + auto receivePath = [&](const nav_msgs::PathConstPtr &path) + { + ROS_INFO_STREAM("Received a path of length " << path->poses.size()); + // tplp->setPlan(path->poses); + }; + auto sub = nh.subscribe("path", 1, receivePath); + + // std::vector plan = std::vector(); + // tplp->setPlan(plan); // tplp->cancel(); + tplp->initialize("path_tracker", &tfBuffer, &costmap); + ros::spin(); ROS_INFO("Bye-bye Path Tracker"); From 28e5d885fd27931e5702ae6d848508c3230aa940 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 21 Apr 2022 17:10:16 +0200 Subject: [PATCH 05/27] Setting up periodic Twist calculator --- src/path_tracker_node.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 46e4360c..e65e696e 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -36,6 +36,19 @@ int main(int argc, char *argv[]) tplp->initialize("path_tracker", &tfBuffer, &costmap); + auto timerCallback = [&](const ros::TimerEvent &) + { + ROS_INFO("Tick"); + /* TODO: + * - Set up a Twist publisher + * - Get the current robot pose from TF + * - Calculate velocity from diff with previous pose + * - publish the Twist + */ + // tplp->computeVelocityCommands(); + }; + ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); + ros::spin(); ROS_INFO("Bye-bye Path Tracker"); From 240e9709052d471a4f39a01ebebffa3c7fd7e113 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 22 Apr 2022 10:14:03 +0200 Subject: [PATCH 06/27] Call computeVelocityCommands in loop with correct arguments --- src/path_tracker_node.cpp | 35 ++++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index e65e696e..8b2fd55e 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -2,6 +2,7 @@ #include #include #include +// #include #include #include @@ -20,37 +21,57 @@ int main(int argc, char *argv[]) tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); + // tf2::TransformListener listener; costmap_2d::Costmap2DROS costmap("empty_costmap", tfBuffer); + std::string map_frame_; + std::string base_link_frame_; + + nh.param("base_link_frame", base_link_frame_, "base_link"); + map_frame_ = costmap.getGlobalFrameID(); //TODO: Not sure if this works if we init the costmap like this + ROS_INFO_STREAM("base_link_frame=" << base_link_frame_ << ", map_frame=" << map_frame_); + + tf2::Stamped prev_transform; + // listener.lookupTransform(map_frame_, base_link_frame_, ros::Time(0), prev_transform); + + ros::Publisher cmd_vel_pub = nh.advertise("cmd_vel", 5); + auto receivePath = [&](const nav_msgs::PathConstPtr &path) { ROS_INFO_STREAM("Received a path of length " << path->poses.size()); - // tplp->setPlan(path->poses); + tplp->setPlan(path->poses); }; auto sub = nh.subscribe("path", 1, receivePath); - // std::vector plan = std::vector(); - // tplp->setPlan(plan); - // tplp->cancel(); - tplp->initialize("path_tracker", &tfBuffer, &costmap); auto timerCallback = [&](const ros::TimerEvent &) { ROS_INFO("Tick"); /* TODO: - * - Set up a Twist publisher * - Get the current robot pose from TF * - Calculate velocity from diff with previous pose * - publish the Twist */ - // tplp->computeVelocityCommands(); + + geometry_msgs::TwistStamped cmd_vel; + geometry_msgs::PoseStamped current_pose; + geometry_msgs::TwistStamped velocity; + std::string message; + + tplp->computeVelocityCommands(current_pose, velocity, cmd_vel, message); + + ROS_INFO_COND(!message.empty(), message.c_str()); + + cmd_vel_pub.publish(cmd_vel.twist); }; ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); ros::spin(); + tplp->cancel(); + ROS_INFO("Bye-bye Path Tracker"); return 0; From b27e50a048c3bb54c5392aa82dcbea0fef8a9743 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 22 Apr 2022 10:52:59 +0200 Subject: [PATCH 07/27] Wait with computing velocities until we have a plan --- src/path_tracker_node.cpp | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 8b2fd55e..e569a91d 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -21,7 +21,6 @@ int main(int argc, char *argv[]) tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); - // tf2::TransformListener listener; costmap_2d::Costmap2DROS costmap("empty_costmap", tfBuffer); @@ -32,8 +31,7 @@ int main(int argc, char *argv[]) map_frame_ = costmap.getGlobalFrameID(); //TODO: Not sure if this works if we init the costmap like this ROS_INFO_STREAM("base_link_frame=" << base_link_frame_ << ", map_frame=" << map_frame_); - tf2::Stamped prev_transform; - // listener.lookupTransform(map_frame_, base_link_frame_, ros::Time(0), prev_transform); + bool has_plan = false; ros::Publisher cmd_vel_pub = nh.advertise("cmd_vel", 5); @@ -41,6 +39,7 @@ int main(int argc, char *argv[]) { ROS_INFO_STREAM("Received a path of length " << path->poses.size()); tplp->setPlan(path->poses); + has_plan = true; }; auto sub = nh.subscribe("path", 1, receivePath); @@ -49,22 +48,25 @@ int main(int argc, char *argv[]) auto timerCallback = [&](const ros::TimerEvent &) { ROS_INFO("Tick"); - /* TODO: - * - Get the current robot pose from TF - * - Calculate velocity from diff with previous pose - * - publish the Twist - */ - geometry_msgs::TwistStamped cmd_vel; - geometry_msgs::PoseStamped current_pose; - geometry_msgs::TwistStamped velocity; - std::string message; + bool goal_reached = tplp->isGoalReached(0.0, 0.0); // The tolerances are ignored by implementation + if (has_plan && goal_reached) + { + geometry_msgs::TwistStamped cmd_vel; + geometry_msgs::PoseStamped current_pose; + geometry_msgs::TwistStamped velocity; + std::string message; - tplp->computeVelocityCommands(current_pose, velocity, cmd_vel, message); + tplp->computeVelocityCommands(current_pose, velocity, cmd_vel, message); // The current_pose and velocty are ignored by implementation - ROS_INFO_COND(!message.empty(), message.c_str()); + ROS_INFO_COND(!message.empty(), message.c_str()); - cmd_vel_pub.publish(cmd_vel.twist); + cmd_vel_pub.publish(cmd_vel.twist); + } + else + { + ROS_INFO_STREAM("Nothing to do: we have " << has_plan << " plan and goal reached = " << goal_reached); + } }; ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); From bc6ae4ca2b678a752fb0523dbae0b38bfaf5c530 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 22 Apr 2022 16:04:57 +0200 Subject: [PATCH 08/27] Some logging on end condition --- src/controller.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/controller.cpp b/src/controller.cpp index 7f3bbbbb..a1b1e75f 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -511,6 +511,16 @@ Controller::UpdateResult Controller::update( // Force target_end_x_vel at the very end of the path // Or when the end velocity is reached. // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose + ROS_DEBUG_STREAM("distance_to_goal: " << distance_to_goal); + ROS_DEBUG_STREAM("target_end_x_vel: " << target_end_x_vel); + ROS_DEBUG_STREAM("new_x_vel: " << new_x_vel); + ROS_DEBUG_STREAM("controller_state_.end_phase_enabled: " << controller_state_.end_phase_enabled); + ROS_DEBUG_STREAM("VELOCITY_EPS: " << VELOCITY_EPS); + ROS_DEBUG_STREAM("distance_to_goal == 0.0: " << distance_to_goal << " == 0 : " << (distance_to_goal == 0.0)); + ROS_DEBUG_STREAM("target_end_x_vel >= VELOCITY_EPS: " << target_end_x_vel << " >= " << VELOCITY_EPS << ". : " << (target_end_x_vel >= VELOCITY_EPS)); + ROS_DEBUG_STREAM("new_x_vel >= target_end_x_vel - VELOCITY_EPS: " << new_x_vel << " >= " << (target_end_x_vel - VELOCITY_EPS) << ". : " << (new_x_vel >= target_end_x_vel - VELOCITY_EPS)); + ROS_DEBUG("----------"); + if ( (distance_to_goal == 0.0 && target_end_x_vel >= VELOCITY_EPS) || (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS && From cce1b61f517cc85e47789766e89f97bb6b10c308 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 22 Apr 2022 16:05:46 +0200 Subject: [PATCH 09/27] Small fixes --- src/path_tracker_node.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index e569a91d..5e98d3f3 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -38,8 +38,7 @@ int main(int argc, char *argv[]) auto receivePath = [&](const nav_msgs::PathConstPtr &path) { ROS_INFO_STREAM("Received a path of length " << path->poses.size()); - tplp->setPlan(path->poses); - has_plan = true; + has_plan = tplp->setPlan(path->poses); }; auto sub = nh.subscribe("path", 1, receivePath); @@ -47,10 +46,8 @@ int main(int argc, char *argv[]) auto timerCallback = [&](const ros::TimerEvent &) { - ROS_INFO("Tick"); - bool goal_reached = tplp->isGoalReached(0.0, 0.0); // The tolerances are ignored by implementation - if (has_plan && goal_reached) + if (has_plan && !goal_reached) { geometry_msgs::TwistStamped cmd_vel; geometry_msgs::PoseStamped current_pose; @@ -70,11 +67,10 @@ int main(int argc, char *argv[]) }; ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); - ros::spin(); - - tplp->cancel(); - - ROS_INFO("Bye-bye Path Tracker"); + while (ros::ok()) + { + ros::spinOnce(); + } return 0; } \ No newline at end of file From 8fe3d0c26b6bf91d7d897e8f5f836785c9d4ca97 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 25 Apr 2022 09:49:36 +0200 Subject: [PATCH 10/27] Make the test path a loop: square with beveled corners --- trajectories/test_path.yaml | 86 ++++++++++++++++++++++++++++++------- 1 file changed, 70 insertions(+), 16 deletions(-) diff --git a/trajectories/test_path.yaml b/trajectories/test_path.yaml index 5a1f7496..e1d7a444 100644 --- a/trajectories/test_path.yaml +++ b/trajectories/test_path.yaml @@ -3,7 +3,7 @@ header: stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' poses: - header: @@ -11,7 +11,7 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' pose: position: x: 0.0 @@ -28,10 +28,10 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' pose: position: - x: 5.0 + x: 14.0 y: 0.0 z: 0.0 orientation: @@ -45,48 +45,102 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' pose: position: - x: 5.0 - y: 5.0 + x: 15.0 + y: 1.0 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.0 - w: 1.0 + z: 0.7071 + w: 0.7071 - header: seq: 3 stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' pose: position: - x: 0.0 - y: 5.0 + x: 15.0 + y: 14.0 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.0 - w: 1.0 + z: 0.7071 + w: 0.7071 - header: seq: 4 stamp: secs: 0 nsecs: 0 - frame_id: '/map' + frame_id: 'map' pose: position: + x: 14.0 + y: 15.0 + z: 0.0 + orientation: x: 0.0 y: 0.0 + z: 1.0 + w: 0.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 1.0 + y: 15.0 z: 0.0 orientation: x: 0.0 y: 0.0 + z: 1.0 + w: 0.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 0.0 + y: 14.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7071 + w: 0.7071 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 0.0 + y: 1.0 z: 0.0 - w: 1.0 \ No newline at end of file + orientation: + x: 0.0 + y: 0.0 + z: -0.7071 + w: 0.7071 + + + From aabe062e7bde4efebf99c1ba45c573ce5c642f50 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 25 Apr 2022 13:49:52 +0200 Subject: [PATCH 11/27] Hanlde finishing a 2nd plan better --- src/path_tracker_node.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 5e98d3f3..5326cd5f 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -62,7 +62,13 @@ int main(int argc, char *argv[]) } else { - ROS_INFO_STREAM("Nothing to do: we have " << has_plan << " plan and goal reached = " << goal_reached); + ROS_DEBUG_STREAM("Nothing to do: we have " << has_plan << " plan and goal reached = " << goal_reached); + } + + if(has_plan && goal_reached) + { + has_plan = false; + ROS_INFO("Goal reached"); } }; ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); From a09cec89b6ce42c17c72667a13cc2a124e8c1e46 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 25 Apr 2022 14:16:47 +0200 Subject: [PATCH 12/27] New test trajectory that is square (but with beveled corners) and loops almost back to 0,0 --- trajectories/beveled_square.yaml | 146 +++++++++++++++++++++++++++++++ trajectories/test_path.yaml | 86 ++++-------------- 2 files changed, 162 insertions(+), 70 deletions(-) create mode 100644 trajectories/beveled_square.yaml diff --git a/trajectories/beveled_square.yaml b/trajectories/beveled_square.yaml new file mode 100644 index 00000000..e1d7a444 --- /dev/null +++ b/trajectories/beveled_square.yaml @@ -0,0 +1,146 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 14.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 15.0 + y: 1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071 + w: 0.7071 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 15.0 + y: 14.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071 + w: 0.7071 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 14.0 + y: 15.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 1.0 + w: 0.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 1.0 + y: 15.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 1.0 + w: 0.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 0.0 + y: 14.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7071 + w: 0.7071 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: 'map' + pose: + position: + x: 0.0 + y: 1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7071 + w: 0.7071 + + + diff --git a/trajectories/test_path.yaml b/trajectories/test_path.yaml index e1d7a444..5a1f7496 100644 --- a/trajectories/test_path.yaml +++ b/trajectories/test_path.yaml @@ -3,7 +3,7 @@ header: stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' poses: - header: @@ -11,7 +11,7 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' pose: position: x: 0.0 @@ -28,10 +28,10 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' pose: position: - x: 14.0 + x: 5.0 y: 0.0 z: 0.0 orientation: @@ -45,102 +45,48 @@ poses: stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' pose: position: - x: 15.0 - y: 1.0 + x: 5.0 + y: 5.0 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.7071 - w: 0.7071 + z: 0.0 + w: 1.0 - header: seq: 3 stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' pose: position: - x: 15.0 - y: 14.0 - z: 0.0 - orientation: x: 0.0 - y: 0.0 - z: 0.7071 - w: 0.7071 - - - header: - seq: 4 - stamp: - secs: 0 - nsecs: 0 - frame_id: 'map' - pose: - position: - x: 14.0 - y: 15.0 + y: 5.0 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 1.0 - w: 0.0 - - - header: - seq: 5 - stamp: - secs: 0 - nsecs: 0 - frame_id: 'map' - pose: - position: - x: 1.0 - y: 15.0 z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 1.0 - w: 0.0 + w: 1.0 - header: - seq: 6 + seq: 4 stamp: secs: 0 nsecs: 0 - frame_id: 'map' + frame_id: '/map' pose: position: - x: 0.0 - y: 14.0 - z: 0.0 - orientation: x: 0.0 y: 0.0 - z: -0.7071 - w: 0.7071 - - - header: - seq: 7 - stamp: - secs: 0 - nsecs: 0 - frame_id: 'map' - pose: - position: - x: 0.0 - y: 1.0 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.7071 - w: 0.7071 - - - + z: 0.0 + w: 1.0 \ No newline at end of file From 495450a846548c14b961bc63dc315f7d52bd0577 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 26 Apr 2022 10:07:20 +0200 Subject: [PATCH 13/27] Clean up ye olde TODOs --- src/path_tracker_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 5326cd5f..d6812605 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -15,8 +15,6 @@ int main(int argc, char *argv[]) auto tplp = new path_tracking_pid::TrackingPidLocalPlanner(); - // Call initialize with empty costmap that we don't update - // Subscribe to Paths, convert those to vector of PoseStamped for setPlan // Call computeVelocities at some configurable rate? tf2_ros::Buffer tfBuffer; From e7103946307bd65a2131d22945e0c28637f07d02 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 29 Apr 2022 10:34:14 +0200 Subject: [PATCH 14/27] Ignore paths of length 0 --- src/path_tracker_node.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index d6812605..eb91b924 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -35,8 +35,15 @@ int main(int argc, char *argv[]) auto receivePath = [&](const nav_msgs::PathConstPtr &path) { - ROS_INFO_STREAM("Received a path of length " << path->poses.size()); - has_plan = tplp->setPlan(path->poses); + if (path->poses.size()) + { + ROS_INFO_STREAM("Setting path of length " << path->poses.size()); + has_plan = tplp->setPlan(path->poses); + } + else + { + ROS_WARN_STREAM("Ignoring path of length " << path->poses.size()); + } }; auto sub = nh.subscribe("path", 1, receivePath); From e0761ee3e9311329baa330b3d4e40605c66daadb Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 29 Apr 2022 14:38:17 +0200 Subject: [PATCH 15/27] Fix catkin_lint -W2 issues --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c8d1e0aa..5e3bc3db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,12 +61,12 @@ catkin_package( ) add_library(${PROJECT_NAME} - src/${PROJECT_NAME}_local_planner.cpp - src/controller.cpp src/calculations.cpp + src/controller.cpp src/details/derivative.cpp src/details/integral.cpp src/details/second_order_lowpass.cpp + src/${PROJECT_NAME}_local_planner.cpp src/visualization.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) @@ -88,8 +88,8 @@ roslint_add_test() install( TARGETS - ${PROJECT_NAME} path_tracker + ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -113,7 +113,7 @@ install( ) if(CATKIN_ENABLE_TESTING) - add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) + add_rostest(test/test_${PROJECT_NAME}.test ARGS rviz:=false reconfigure:=false) catkin_add_gtest(unittests test/unittests/test_calculations.cpp test/unittests/test_derivative.cpp From 8e697a8bca7d1cd8bfb8c1b2f24f48efa639e5fc Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 29 Apr 2022 15:23:25 +0200 Subject: [PATCH 16/27] Listen to SegmentPath and slice them at the end befor using --- CMakeLists.txt | 2 ++ package.xml | 1 + src/path_tracker_node.cpp | 28 +++++++++++++++++++++++++--- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e3bc3db..2702d0f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs + amr_road_network_msgs dynamic_reconfigure geometry_msgs mbf_costmap_core @@ -46,6 +47,7 @@ catkin_package( CATKIN_DEPENDS actionlib actionlib_msgs + amr_road_network_msgs dynamic_reconfigure geometry_msgs mbf_costmap_core diff --git a/package.xml b/package.xml index ed4f7197..9cec1e50 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ message_runtime actionlib actionlib_msgs + amr_road_network_msgs dynamic_reconfigure geometry_msgs mbf_costmap_core diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index eb91b924..3d645975 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -2,7 +2,7 @@ #include #include #include -// #include +#include #include #include @@ -42,10 +42,32 @@ int main(int argc, char *argv[]) } else { - ROS_WARN_STREAM("Ignoring path of length " << path->poses.size()); + ROS_WARN_STREAM("Ignoring path of length, cancelling current" << path->poses.size()); + tplp->cancel(); } }; - auto sub = nh.subscribe("path", 1, receivePath); + auto path_sub = nh.subscribe("path", 1, receivePath); + + auto receiveSegmentPath = [&](const amr_road_network_msgs::SegmentPathConstPtr &path) + { + if (path->waypoints.poses.size()) + { + ROS_INFO_STREAM("Received path of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); + // Starting and Ending iterators + auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. + auto end = path->waypoints.poses.begin() + path->target_index + 1; + + std::vector sliced_path(path->target_index + 1); + copy(start, end, sliced_path.begin()); + + has_plan = tplp->setPlan(sliced_path); + } + else + { + ROS_WARN_STREAM("Ignoring path of length " << path->waypoints.poses.size()); + } + }; + auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath); tplp->initialize("path_tracker", &tfBuffer, &costmap); From aae494f04a344c237e5ba60bf3a5d39100e34b6a Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 29 Apr 2022 16:12:05 +0200 Subject: [PATCH 17/27] Skip the first bit of the path and only set plan if there's anything left --- src/path_tracker_node.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 3d645975..6eeaedbe 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -54,17 +54,27 @@ int main(int argc, char *argv[]) { ROS_INFO_STREAM("Received path of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); // Starting and Ending iterators - auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. + int skip = 15; + auto start = path->waypoints.poses.begin() + skip; // TODO: offset to skip the poses behind the robot. auto end = path->waypoints.poses.begin() + path->target_index + 1; - std::vector sliced_path(path->target_index + 1); + std::vector sliced_path(path->target_index - skip + 1); copy(start, end, sliced_path.begin()); - has_plan = tplp->setPlan(sliced_path); + if (sliced_path.size()) + { + has_plan = tplp->setPlan(sliced_path); + } + else + { + ROS_WARN_STREAM("Ignoring sliced path of length " << sliced_path.size()); + tplp->cancel(); + } } else { ROS_WARN_STREAM("Ignoring path of length " << path->waypoints.poses.size()); + tplp->cancel(); } }; auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath); From c3f8d421f233adf246d6bc327f1e48e1c709c09e Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 08:59:23 +0200 Subject: [PATCH 18/27] Bit better logging --- src/path_tracker_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 6eeaedbe..6fbe4e29 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -50,9 +50,10 @@ int main(int argc, char *argv[]) auto receiveSegmentPath = [&](const amr_road_network_msgs::SegmentPathConstPtr &path) { + ROS_INFO_STREAM("Received SegmentPath"); if (path->waypoints.poses.size()) { - ROS_INFO_STREAM("Received path of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); + ROS_INFO_STREAM("Received SegmentPath of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); // Starting and Ending iterators int skip = 15; auto start = path->waypoints.poses.begin() + skip; // TODO: offset to skip the poses behind the robot. From 03f2fa323ecc1a66ccd4b196db7132d5eb4b2adf Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 08:59:59 +0200 Subject: [PATCH 19/27] Cancel tracker when sliced path has length 0 --- src/path_tracker_node.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 6fbe4e29..c209753d 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -68,14 +68,20 @@ int main(int argc, char *argv[]) } else { - ROS_WARN_STREAM("Ignoring sliced path of length " << sliced_path.size()); - tplp->cancel(); + ROS_WARN_STREAM("Ignoring sliced SegmentPath of length " << sliced_path.size()); + if (has_plan) + { + tplp->cancel(); + } } } else { - ROS_WARN_STREAM("Ignoring path of length " << path->waypoints.poses.size()); - tplp->cancel(); + ROS_WARN_STREAM("Ignoring SegmentPath of length " << path->waypoints.poses.size()); + if(has_plan) + { + tplp->cancel(); + } } }; auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath); From 65e89a822cbb14b885b80fe97dacd8de3233bb74 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 09:02:44 +0200 Subject: [PATCH 20/27] WIP: Skip the first 15 waypoints The segment planner prepends these for use in the PLC but we don't need this This fixed index should be replaced by looking at our current pose and only leaving the poses after that --- src/path_tracker_node.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index c209753d..e4f70b2a 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -56,10 +56,14 @@ int main(int argc, char *argv[]) ROS_INFO_STREAM("Received SegmentPath of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); // Starting and Ending iterators int skip = 15; - auto start = path->waypoints.poses.begin() + skip; // TODO: offset to skip the poses behind the robot. + auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. + if(path->target_index > skip) + { + start = start + skip; + } auto end = path->waypoints.poses.begin() + path->target_index + 1; - std::vector sliced_path(path->target_index - skip + 1); + std::vector sliced_path(end - start); copy(start, end, sliced_path.begin()); if (sliced_path.size()) From 72e558897062b7adaae324e7cbc753133333b50d Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 10:30:39 +0200 Subject: [PATCH 21/27] Publish the sliced path for debugging --- src/path_tracker_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index e4f70b2a..406323fc 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -11,6 +11,7 @@ int main(int argc, char *argv[]) { ros::init(argc, argv, "path_tracker"); ros::NodeHandle nh; + ros::NodeHandle pnh("~"); ROS_INFO("Hello Path Tracker"); auto tplp = new path_tracking_pid::TrackingPidLocalPlanner(); @@ -47,6 +48,7 @@ int main(int argc, char *argv[]) } }; auto path_sub = nh.subscribe("path", 1, receivePath); + auto sliced_path_pub = pnh.advertise("sliced_path", 1); auto receiveSegmentPath = [&](const amr_road_network_msgs::SegmentPathConstPtr &path) { @@ -65,6 +67,10 @@ int main(int argc, char *argv[]) std::vector sliced_path(end - start); copy(start, end, sliced_path.begin()); + nav_msgs::Path sliced_nav_path; + sliced_nav_path.header = path->waypoints.header; + sliced_nav_path.poses = sliced_path; + sliced_path_pub.publish(sliced_nav_path); if (sliced_path.size()) { From f6f24f37f857bf942cb227706711358911821574 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 11:21:01 +0200 Subject: [PATCH 22/27] Log more info about path slicing --- src/path_tracker_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 406323fc..9d56613f 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -59,9 +59,11 @@ int main(int argc, char *argv[]) // Starting and Ending iterators int skip = 15; auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. + int start_index = 0; if(path->target_index > skip) { start = start + skip; + start_index = start_index + skip; } auto end = path->waypoints.poses.begin() + path->target_index + 1; @@ -74,6 +76,7 @@ int main(int argc, char *argv[]) if (sliced_path.size()) { + ROS_INFO_STREAM("Setting sliced plan of length " << sliced_path.size() << " starting at " << start_index); has_plan = tplp->setPlan(sliced_path); } else From 2b2c27377c4a9d301daaf1188200d7583921bfe6 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 2 May 2022 11:21:35 +0200 Subject: [PATCH 23/27] Do not cancel plan when getting 0-lenght path, not handled well --- src/path_tracker_node.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 9d56613f..6a879679 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -82,19 +82,19 @@ int main(int argc, char *argv[]) else { ROS_WARN_STREAM("Ignoring sliced SegmentPath of length " << sliced_path.size()); - if (has_plan) - { - tplp->cancel(); - } + // if (has_plan) + // { + // tplp->cancel(); + // } } } else { ROS_WARN_STREAM("Ignoring SegmentPath of length " << path->waypoints.poses.size()); - if(has_plan) - { - tplp->cancel(); - } + // if(has_plan) + // { + // tplp->cancel(); + // } } }; auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath); From e2cf50e117faef3820bd037a6824d61d0911861d Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 3 May 2022 13:16:21 +0200 Subject: [PATCH 24/27] Slice only points after our current pose --- src/path_tracker_node.cpp | 68 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 67 insertions(+), 1 deletion(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 6a879679..57051661 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -5,7 +5,64 @@ #include #include #include +#include +#include +inline double translation_distance(const geometry_msgs::PoseStamped a, const geometry_msgs::PoseStamped b) +{ + assert(a.header.frame_id == b.header.frame_id); + return hypot(b.pose.position.x - a.pose.position.x, b.pose.position.y - a.pose.position.y); +} + +inline double calculate_yaw(geometry_msgs::Quaternion quaternion) +{ + // calculate theta + double roll, pitch, yaw; + tf::Quaternion q = tf::Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w); + tf::Matrix3x3(q).getRPY(roll, pitch, yaw); + return yaw; +} + +inline double rotation_distance(const geometry_msgs::PoseStamped a, const geometry_msgs::PoseStamped b) +{ + assert(a.header.frame_id == b.header.frame_id); + return calculate_yaw(b.pose.orientation) - calculate_yaw(a.pose.orientation); +} + +uint closestIndex(geometry_msgs::PoseStamped global_pose_msg, std::vector global_plan_) +{ + // Step 1: Find the (index of the) waypoint closest to our current pose (global_pose_msg). + // Stop searching further when the distance starts increasing + + size_t start_index = 0; + + // find waypoint closest to robot position + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < global_plan_.size(); i++) + { + auto new_trans_dist = translation_distance(global_pose_msg, global_plan_[i]); + auto new_rot_dist = rotation_distance(global_pose_msg, global_plan_[i]); + ROS_DEBUG_STREAM("extractWaypoints: i=" << i << ", new_trans_distance = " << new_trans_dist << ", new_rot_distance = " << new_rot_dist << ", min_dist = " << min_dist); + if (new_trans_dist <= min_dist) + { + if (abs(new_rot_dist) < 0.1) // goal_tolerance_rotation_ was a variable + { + start_index = i; + min_dist = new_trans_dist; + } + else + { + ROS_DEBUG_STREAM("Point " << i << " at x=" << global_plan_[i].pose.position.x << ", y=" << global_plan_[i].pose.position.y << " is close in translation but has a different orientation (dist:" << new_rot_dist << ") so not actually close"); + } + } + else + { + ROS_DEBUG_STREAM("Path poses are getting further away, so closest has been found"); + break; // Distance is getting bigger again, stop searching + } + } + return start_index; +} int main(int argc, char *argv[]) { @@ -59,7 +116,16 @@ int main(int argc, char *argv[]) // Starting and Ending iterators int skip = 15; auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. - int start_index = 0; + + + geometry_msgs::PoseStamped current_pose; + if (!costmap.getRobotPose(current_pose)) + { + ROS_WARN("Could not retrieve up to date robot pose from costmap!"); + return; + } + int start_index = closestIndex(current_pose, path->waypoints.poses); + if(path->target_index > skip) { start = start + skip; From 20c0f37d3417fe7892d9205fbe19a7026353378e Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 3 May 2022 15:45:21 +0200 Subject: [PATCH 25/27] Run with 2 threads so that .cancel be handled in a separate thread, as it desires --- src/path_tracker_node.cpp | 35 +++++++++++++++++------------------ 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index 57051661..d53a252b 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -113,18 +113,18 @@ int main(int argc, char *argv[]) if (path->waypoints.poses.size()) { ROS_INFO_STREAM("Received SegmentPath of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); - // Starting and Ending iterators - int skip = 15; - auto start = path->waypoints.poses.begin(); // TODO: offset to skip the poses behind the robot. - - + geometry_msgs::PoseStamped current_pose; if (!costmap.getRobotPose(current_pose)) { ROS_WARN("Could not retrieve up to date robot pose from costmap!"); return; } - int start_index = closestIndex(current_pose, path->waypoints.poses); + + // Starting and Ending iterators + int skip = closestIndex(current_pose, path->waypoints.poses); + auto start = path->waypoints.poses.begin(); + int start_index = 0; if(path->target_index > skip) { @@ -148,19 +148,19 @@ int main(int argc, char *argv[]) else { ROS_WARN_STREAM("Ignoring sliced SegmentPath of length " << sliced_path.size()); - // if (has_plan) - // { - // tplp->cancel(); - // } + if (has_plan) + { + tplp->cancel(); + } } } else { ROS_WARN_STREAM("Ignoring SegmentPath of length " << path->waypoints.poses.size()); - // if(has_plan) - // { - // tplp->cancel(); - // } + if(has_plan) + { + tplp->cancel(); + } } }; auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath); @@ -196,10 +196,9 @@ int main(int argc, char *argv[]) }; ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); - while (ros::ok()) - { - ros::spinOnce(); - } + ros::AsyncSpinner spinner(2); + spinner.start(); + ros::waitForShutdown(); return 0; } \ No newline at end of file From d9f29082c4ba367a7a1582b90ba987644096fd8c Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 3 May 2022 15:55:29 +0200 Subject: [PATCH 26/27] Improve logging: clearer and only the relevant bits --- src/path_tracker_node.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index d53a252b..f8afe2af 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -109,10 +109,10 @@ int main(int argc, char *argv[]) auto receiveSegmentPath = [&](const amr_road_network_msgs::SegmentPathConstPtr &path) { - ROS_INFO_STREAM("Received SegmentPath"); + ROS_DEBUG_STREAM("Received SegmentPath"); if (path->waypoints.poses.size()) { - ROS_INFO_STREAM("Received SegmentPath of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); + ROS_DEBUG_STREAM("Received SegmentPath of length " << path->waypoints.poses.size() << " with target at index " << path->target_index); geometry_msgs::PoseStamped current_pose; if (!costmap.getRobotPose(current_pose)) @@ -132,6 +132,7 @@ int main(int argc, char *argv[]) start_index = start_index + skip; } auto end = path->waypoints.poses.begin() + path->target_index + 1; + int end_index = path->target_index + 1; std::vector sliced_path(end - start); copy(start, end, sliced_path.begin()); @@ -142,7 +143,7 @@ int main(int argc, char *argv[]) if (sliced_path.size()) { - ROS_INFO_STREAM("Setting sliced plan of length " << sliced_path.size() << " starting at " << start_index); + ROS_INFO_STREAM("Setting path of length " << sliced_path.size() << " from " << path->waypoints.poses.size() << " waypoints, start at " << start_index << ", end at " << end_index); has_plan = tplp->setPlan(sliced_path); } else From f947ad78bc87f223533af3cb7dd424b7e9f13191 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Wed, 4 May 2022 09:38:42 +0200 Subject: [PATCH 27/27] Not cancelling after receiving a 0-length path works better apparently --- src/path_tracker_node.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/path_tracker_node.cpp b/src/path_tracker_node.cpp index f8afe2af..c87032b2 100644 --- a/src/path_tracker_node.cpp +++ b/src/path_tracker_node.cpp @@ -149,19 +149,19 @@ int main(int argc, char *argv[]) else { ROS_WARN_STREAM("Ignoring sliced SegmentPath of length " << sliced_path.size()); - if (has_plan) - { - tplp->cancel(); - } + // if (has_plan) + // { + // tplp->cancel(); + // } } } else { ROS_WARN_STREAM("Ignoring SegmentPath of length " << path->waypoints.poses.size()); - if(has_plan) - { - tplp->cancel(); - } + // if(has_plan) + // { + // tplp->cancel(); + // } } }; auto segment_sub = nh.subscribe("segment_path", 1, receiveSegmentPath);