Feat/waypoint manager #645
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -57,7 +57,4 @@ qtcreator-* | |
| # Emacs | ||
| .#* | ||
|
|
||
| # End of https://www.gitignore.io/api/ros | ||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -195,12 +195,12 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { | |
| } | ||
|
|
||
| Eigen::Vector6d ReferenceFilterNode::fill_reference_goal( | ||
| const geometry_msgs::msg::Pose& goal) { | ||
| double x{goal.position.x}; | ||
| double y{goal.position.y}; | ||
| double z{goal.position.z}; | ||
|
|
||
| const auto& o = goal.orientation; | ||
| Eigen::Quaterniond q(o.w, o.x, o.y, o.z); | ||
| Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); | ||
| double roll{euler_angles(0)}; | ||
|
|
@@ -231,6 +231,43 @@ vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { | |
| return feedback_msg; | ||
| } | ||
|
|
||
| Eigen::Vector6d ReferenceFilterNode::apply_mode_logic( | ||
| const Eigen::Vector6d& r_in, | ||
| uint8_t mode) { | ||
| Eigen::Vector6d r_out = r_in; | ||
|
|
||
| switch (mode) { | ||
| case vortex_msgs::msg::Waypoint::FULL_POSE: | ||
| break; | ||
|
|
||
| case vortex_msgs::msg::Waypoint::ONLY_POSITION: | ||
| r_out(3) = x_(3); | ||
| r_out(4) = x_(4); | ||
| r_out(5) = x_(5); | ||
| break; | ||
|
Comment on lines
+243
to
+247
Contributor
There was a problem hiding this comment. Why isn't logic like
Contributor
There was a problem hiding this comment. I do think its fine that the wp manager/guidance controls which mode and so on, but the controller(s) should probably also know what mode it is performing
Contributor
Author
There was a problem hiding this comment. The locking is desired for this use case.
Contributor
Author
There was a problem hiding this comment. I agree that the mode logic could be handled in the controller, but this setup currently works as a MVP, and fits nicely within the scope of the action.
Contributor
There was a problem hiding this comment. locking the drone to a desired orientation could be something that the state machine requests and guidance system does i don't see the point of the (DP) controller differenciating between locking orientation and locking pose it is the same thing but simpler, albeit could be better tuned to some extent if we separate them. The use case in this situation is more that we do not really want to change the position which would require the state machine to remember its last waypoint, recover the (x,y,z) coordinates and then send the same waypoint but with changed orientation which seemes tedious. Same goes for position only vice versa. The thing that could be something to discuss is if we want the state machine to only communicate its desires with the orchistrator/manager node, otherwise we could make the state machine directly send requests to DP guidance about locking orientation.
Member
There was a problem hiding this comment. Possible to just let each dof be addressable (i.e. Eigen::Index, strongly typed) and let whatever thing thats managing task execution lock whatever axes as required? (could also alias "common" things like position only, orientation only, z only, etc, etc). That way you keep the controller "dumb" in the sense that it has no knowledge of "mode", just "which axes are currently constrained". (I don't know anything about the current dp code so correct me if this is already the case 🔥) |
||
|
|
||
| case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { | ||
| double dx = r_in(0) - x_(0); | ||
| double dy = r_in(1) - x_(1); | ||
|
|
||
| double forward_heading = std::atan2(dy, dx); | ||
|
|
||
| r_out(3) = 0.0; | ||
| r_out(4) = 0.0; | ||
| r_out(5) = vortex::utils::math::ssa(forward_heading); | ||
| break; | ||
| } | ||
|
|
||
| case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: | ||
| r_out(0) = x_(0); | ||
| r_out(1) = x_(1); | ||
| r_out(2) = x_(2); | ||
| break; | ||
| } | ||
|
|
||
| return r_out; | ||
| } | ||
|
|
||
| void ReferenceFilterNode::execute( | ||
| const std::shared_ptr<rclcpp_action::ServerGoalHandle< | ||
| vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) { | ||
|
|
@@ -243,9 +280,21 @@ void ReferenceFilterNode::execute( | |
|
|
||
| x_ = fill_reference_state(); | ||
|
|
||
| const geometry_msgs::msg::Pose goal = | ||
| goal_handle->get_goal()->waypoint.pose; | ||
| uint8_t mode = goal_handle->get_goal()->waypoint.mode; | ||
| double convergence_threshold = | ||
| goal_handle->get_goal()->convergence_threshold; | ||
|
Comment on lines
+283
to
+287
Contributor
There was a problem hiding this comment. I would try to use |
||
|
|
||
| if (convergence_threshold <= 0.0) { | ||
| convergence_threshold = 0.1; | ||
| spdlog::warn( | ||
| "ReferenceFilter: Invalid convergence_threshold received (<= 0). " | ||
| "Using default 0.1"); | ||
| } | ||
|
|
||
| Eigen::Vector6d r_temp = fill_reference_goal(goal); | ||
| r_ = apply_mode_logic(r_temp, mode); | ||
|
|
||
| auto feedback = std::make_shared< | ||
| vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); | ||
|
|
@@ -274,12 +323,12 @@ void ReferenceFilterNode::execute( | |
|
|
||
| vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); | ||
|
|
||
| feedback->reference = feedback_msg; | ||
|
|
||
| goal_handle->publish_feedback(feedback); | ||
| reference_pub_->publish(feedback_msg); | ||
|
|
||
| if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { | ||
| result->success = true; | ||
| goal_handle->succeed(result); | ||
| x_.head(6) = r_.head(6); | ||
|
|
@@ -292,6 +341,17 @@ void ReferenceFilterNode::execute( | |
|
|
||
| loop_rate.sleep(); | ||
| } | ||
| if (!rclcpp::ok() && goal_handle->is_active()) { | ||
| auto result = std::make_shared< | ||
| vortex_msgs::action::ReferenceFilterWaypoint::Result>(); | ||
| result->success = false; | ||
|
|
||
| try { | ||
| goal_handle->abort(result); | ||
| } catch (...) { | ||
| // Ignore exceptions during shutdown | ||
| } | ||
| } | ||
| } | ||
|
|
||
| RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) | ||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,2 @@ | ||
| # Ignored temporarily to skip building while debugging yasmin upgrade | ||
| # Remove this file to include the package in colcon builds again |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,69 @@ | ||
| cmake_minimum_required(VERSION 3.8) | ||
| project(waypoint_manager) | ||
|
|
||
| if(NOT CMAKE_CXX_STANDARD) | ||
| set(CMAKE_CXX_STANDARD 20) | ||
| endif() | ||
|
|
||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() | ||
|
|
||
| find_package(ament_cmake REQUIRED) | ||
| find_package(rclcpp REQUIRED) | ||
| find_package(rclcpp_action REQUIRED) | ||
| find_package(rclcpp_components REQUIRED) | ||
| find_package(vortex_msgs REQUIRED) | ||
| find_package(vortex_utils REQUIRED) | ||
| find_package(tf2_geometry_msgs REQUIRED) | ||
| find_package(spdlog REQUIRED) | ||
| find_package(fmt REQUIRED) | ||
|
|
||
| include_directories(include) | ||
|
|
||
| set(LIB_NAME "${PROJECT_NAME}_component") | ||
|
|
||
| add_library(${LIB_NAME} SHARED | ||
| src/waypoint_manager_ros.cpp) | ||
|
|
||
| ament_target_dependencies(${LIB_NAME} PUBLIC | ||
| rclcpp | ||
| rclcpp_components | ||
| rclcpp_action | ||
| vortex_msgs | ||
| vortex_utils | ||
| tf2_geometry_msgs | ||
| spdlog | ||
| fmt | ||
| ) | ||
|
|
||
| rclcpp_components_register_node( | ||
| ${LIB_NAME} | ||
| PLUGIN "WaypointManagerNode" | ||
| EXECUTABLE ${PROJECT_NAME}_node | ||
| ) | ||
|
|
||
| ament_export_targets(export_${LIB_NAME}) | ||
|
|
||
| install(TARGETS ${LIB_NAME} | ||
| EXPORT export_${LIB_NAME} | ||
| ARCHIVE DESTINATION lib | ||
| LIBRARY DESTINATION lib | ||
| RUNTIME DESTINATION bin | ||
| ) | ||
|
|
||
| install( | ||
| DIRECTORY include/ | ||
| DESTINATION include | ||
| ) | ||
|
|
||
| install(DIRECTORY | ||
| launch | ||
| DESTINATION share/${PROJECT_NAME}/ | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| add_subdirectory(test) | ||
| endif() | ||
|
|
||
| ament_package() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why is stamped removed?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The function did not use the header, so no need to pass more than what is required
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
vortex utils ros conversion here instead?