diff --git a/mesh_controller/CMakeLists.txt b/mesh_controller/CMakeLists.txt index adbf2e6b..28f7b4e9 100644 --- a/mesh_controller/CMakeLists.txt +++ b/mesh_controller/CMakeLists.txt @@ -1,54 +1,35 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(mesh_controller) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) +find_package(ament_cmake_ros REQUIRED) +find_package(example_interfaces REQUIRED) +find_package(mbf_mesh_core REQUIRED) +find_package(mbf_msgs REQUIRED) +find_package(mbf_utility REQUIRED) +find_package(mesh_map REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - mbf_mesh_core - mbf_utility - mbf_msgs - mesh_map - tf2_geometry_msgs - dynamic_reconfigure -) - -generate_dynamic_reconfigure_options( - cfg/MeshController.cfg -) - -catkin_package( - LIBRARIES mesh_controller - CATKIN_DEPENDS roscpp mbf_mesh_core mbf_msgs mesh_map tf2_geometry_msgs dynamic_reconfigure -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - src/mesh_controller.cpp -) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg -) +pluginlib_export_plugin_description_file(mbf_mesh_core mesh_controller.xml) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) +add_library(${PROJECT_NAME} src/mesh_controller.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_CONTROLLER_BUILDING_LIBRARY") +ament_target_dependencies(${PROJECT_NAME} example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs) +install(DIRECTORY include DESTINATION include) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(FILES mesh_controller.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +ament_export_dependencies(example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs) +ament_package() \ No newline at end of file diff --git a/mesh_controller/COLCON_IGNORE b/mesh_controller/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/mesh_controller/include/mesh_controller/mesh_controller.h b/mesh_controller/include/mesh_controller/mesh_controller.h index 1a626e6b..3796b03e 100644 --- a/mesh_controller/include/mesh_controller/mesh_controller.h +++ b/mesh_controller/include/mesh_controller/mesh_controller.h @@ -36,10 +36,10 @@ #define MESH_NAVIGATION__MESH_CONTROLLER_H #include -#include -#include +#include +#include #include -#include +#include namespace mesh_controller { @@ -48,7 +48,7 @@ class MeshController : public mbf_mesh_core::MeshController public: //! shared pointer typedef to simplify pointer access of the mesh controller - typedef boost::shared_ptr Ptr; + typedef std::shared_ptr Ptr; /** * @brief Constructor @@ -69,9 +69,9 @@ class MeshController : public mbf_mesh_core::MeshController * @param message Detailed outcome as string message * @return An mbf_msgs/ExePathResult outcome code */ - virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, - const geometry_msgs::TwistStamped& velocity, - geometry_msgs::TwistStamped& cmd_vel, std::string& message); + virtual uint32_t computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose, + const geometry_msgs::msg::TwistStamped& velocity, + geometry_msgs::msg::TwistStamped& cmd_vel, std::string& message) override; /** * @brief Checks if the robot reached to goal pose @@ -82,20 +82,32 @@ class MeshController : public mbf_mesh_core::MeshController * be partly accepted as reached goal * @return true if the goal is reached */ - virtual bool isGoalReached(double dist_tolerance, double angle_tolerance); + virtual bool isGoalReached(double dist_tolerance, double angle_tolerance) override; /** * @brief Sets the current plan to follow, it also sets the vector field * @param plan The plan to follow * @return true if the plan was set successfully, false otherwise */ - virtual bool setPlan(const std::vector& plan); + virtual bool setPlan(const std::vector& plan) override; /** * @brief Requests the planner to cancel, e.g. if it takes too much time * @return True if cancel has been successfully requested, false otherwise */ - virtual bool cancel(); + virtual bool cancel() override; + + /** + * @brief Initializes the controller plugin with a name, a tf pointer and a mesh map pointer + * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace + * @param tf_ptr A shared pointer to a transformation buffer + * @param mesh_map_ptr A shared pointer to the mesh map + * @return true if the plugin has been initialized successfully + */ + virtual bool initialize(const std::string& plugin_name, + const std::shared_ptr& tf_ptr, + const std::shared_ptr& mesh_map_ptr, + const rclcpp::Node::SharedPtr& node) override; /** * Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector @@ -103,7 +115,7 @@ class MeshController : public mbf_mesh_core::MeshController * @return direction normal vector */ mesh_map::Normal poseToDirectionVector( - const geometry_msgs::PoseStamped& pose, + const geometry_msgs::msg::PoseStamped& pose, const tf2::Vector3& axis=tf2::Vector3(1,0,0)); @@ -113,7 +125,7 @@ class MeshController : public mbf_mesh_core::MeshController * @return position vector */ mesh_map::Vector poseToPositionVector( - const geometry_msgs::PoseStamped& pose); + const geometry_msgs::msg::PoseStamped& pose); /** * A normal distribution / gaussian function @@ -141,52 +153,52 @@ class MeshController : public mbf_mesh_core::MeshController /** * @brief reconfigure callback function which is called if a dynamic reconfiguration were triggered. */ - void reconfigureCallback(mesh_controller::MeshControllerConfig& cfg, uint32_t level); - - /** - * @brief Initializes the controller plugin with a name, a tf pointer and a mesh map pointer - * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace - * @param tf_ptr A shared pointer to a transformation buffer - * @param mesh_map_ptr A shared pointer to the mesh map - * @return true if the plugin has been initialized successfully - */ - virtual bool initialize(const std::string& plugin_name, const boost::shared_ptr& tf_ptr, - const boost::shared_ptr& mesh_map_ptr); + rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); private: + //! shared pointer to node in which this plugin runs + rclcpp::Node::SharedPtr node_; + + //! the user defined plugin name + std::string name_; //! shared pointer to the used mesh map - boost::shared_ptr map_ptr; + std::shared_ptr map_ptr_; //! the current set plan - vector current_plan; + vector current_plan_; //! the goal and robot pose - mesh_map::Vector goal_pos, robot_pos; + mesh_map::Vector goal_pos_, robot_pos_; //! the goal's and robot's orientation - mesh_map::Normal goal_dir, robot_dir; + mesh_map::Normal goal_dir_, robot_dir_; //! The triangle on which the robot is located - lvr2::OptionalFaceHandle current_face; + lvr2::OptionalFaceHandle current_face_; //! The vector field to the goal. - lvr2::DenseVertexMap vector_map; - - //! shared pointer to dynamic reconfigure server - boost::shared_ptr> reconfigure_server_ptr; - - //! dynamic reconfigure callback function binding - dynamic_reconfigure::Server::CallbackType config_callback; - - //! current mesh controller configuration - MeshControllerConfig config; + lvr2::DenseVertexMap vector_map_; //! publishes the angle between the robots orientation and the goal vector field for debug purposes - ros::Publisher angle_pub; + rclcpp::Publisher::SharedPtr angle_pub_; //! flag to handle cancel requests - std::atomic_bool cancel_requested; + std::atomic_bool cancel_requested_; + + // handle of callback for changing parameters dynamically + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; + + struct { + double max_lin_velocity = 1.0; + double max_ang_velocity = 0.5; + double arrival_fading = 0.5; + double ang_vel_factor = 1.0; + double lin_vel_factor = 1.0; + double max_angle = 20.0; + double max_search_radius = 0.4; + double max_search_distance = 0.4; + } config_; }; } /* namespace mesh_controller */ diff --git a/mesh_controller/package.xml b/mesh_controller/package.xml index 4347b916..35c8aef2 100644 --- a/mesh_controller/package.xml +++ b/mesh_controller/package.xml @@ -1,5 +1,5 @@ - + mesh_controller 1.0.1 The mesh_controller package @@ -9,17 +9,15 @@ Sebastian Pütz Sabrina Frohn - catkin - - roscpp mbf_mesh_core - mbf_utility mbf_msgs + mbf_utility mesh_map + rclcpp tf2_geometry_msgs - dynamic_reconfigure + example_interfaces - + ament_cmake diff --git a/mesh_controller/src/mesh_controller.cpp b/mesh_controller/src/mesh_controller.cpp index 23c54bfe..2e186ac3 100644 --- a/mesh_controller/src/mesh_controller.cpp +++ b/mesh_controller/src/mesh_controller.cpp @@ -37,13 +37,11 @@ #include #include -#include -#include +#include #include #include -#include -#include -#include +#include +#include #include PLUGINLIB_EXPORT_CLASS(mesh_controller::MeshController, mbf_mesh_core::MeshController); @@ -66,142 +64,143 @@ MeshController::~MeshController() { } -uint32_t MeshController::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, - const geometry_msgs::TwistStamped& velocity, - geometry_msgs::TwistStamped& cmd_vel, std::string& message) +uint32_t MeshController::computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose, + const geometry_msgs::msg::TwistStamped& velocity, + geometry_msgs::msg::TwistStamped& cmd_vel, + std::string& message) { - const auto& mesh = map_ptr->mesh(); + const auto& mesh = map_ptr_->mesh(); - robot_pos = poseToPositionVector(pose); - robot_dir = poseToDirectionVector(pose); + robot_pos_ = poseToPositionVector(pose); + robot_dir_ = poseToDirectionVector(pose); std::array bary_coords; std::array vertices; - if (!current_face) + if (!current_face_) { // initially search current face on complete map - if (auto search_res_opt = map_ptr->searchContainingFace( - robot_pos, config.max_search_distance)) + if (auto search_res_opt = map_ptr_->searchContainingFace( + robot_pos_, config_.max_search_distance)) { auto search_res = *search_res_opt; - current_face = std::get<0>(search_res); + current_face_ = std::get<0>(search_res); vertices = std::get<1>(search_res); bary_coords = std::get<2>(search_res); // project position onto surface - robot_pos = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); + robot_pos_ = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); } else { // no corresponding face has been found - return mbf_msgs::ExePathResult::OUT_OF_MAP; + return mbf_msgs::action::ExePath::Result::OUT_OF_MAP; } } else // current face is set { - lvr2::FaceHandle face = current_face.unwrap(); + lvr2::FaceHandle face = current_face_.unwrap(); vertices = mesh.getVertexPositionsOfFace(face); - DEBUG_CALL(map_ptr->publishDebugFace(face, mesh_map::color(1, 1, 1), "current_face");) - DEBUG_CALL(map_ptr->publishDebugPoint(robot_pos, mesh_map::color(1, 1, 1), "robot_position");) + DEBUG_CALL(map_ptr_->publishDebugFace(face, mesh_map::color(1, 1, 1), "current_face");) + DEBUG_CALL(map_ptr_->publishDebugPoint(robot_pos_, mesh_map::color(1, 1, 1), "robot_position");) float dist_to_surface; // check whether or not the position matches the current face // if not search for new current face if (mesh_map::projectedBarycentricCoords( - robot_pos, vertices, bary_coords, dist_to_surface) - && dist_to_surface < config.max_search_distance) + robot_pos_, vertices, bary_coords, dist_to_surface) + && dist_to_surface < config_.max_search_distance) { // current position is located inside and close enough to the face - DEBUG_CALL(map_ptr->publishDebugPoint(robot_pos, mesh_map::color(0, 0, 1), "current_position");) + DEBUG_CALL(map_ptr_->publishDebugPoint(robot_pos_, mesh_map::color(0, 0, 1), "current_position");) } - else if (auto search_res_opt = map_ptr->searchNeighbourFaces( - robot_pos, face, config.max_search_radius, config.max_search_distance)) + else if (auto search_res_opt = map_ptr_->searchNeighbourFaces( + robot_pos_, face, config_.max_search_radius, config_.max_search_distance)) { // new face has been found out of the neighbour faces of the current face // update variables to new face auto search_res = *search_res_opt; - current_face = face = std::get<0>(search_res); + current_face_ = face = std::get<0>(search_res); vertices = std::get<1>(search_res); bary_coords = std::get<2>(search_res); - robot_pos = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); - DEBUG_CALL(map_ptr->publishDebugFace(face, mesh_map::color(1, 0.5, 0), "search_neighbour_face");) - DEBUG_CALL(map_ptr->publishDebugPoint(robot_pos, mesh_map::color(0, 0, 1), "search_neighbour_pos");) + robot_pos_ = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); + DEBUG_CALL(map_ptr_->publishDebugFace(face, mesh_map::color(1, 0.5, 0), "search_neighbour_face");) + DEBUG_CALL(map_ptr_->publishDebugPoint(robot_pos_, mesh_map::color(0, 0, 1), "search_neighbour_pos");) } - else if(auto search_res_opt = map_ptr->searchContainingFace( - robot_pos, config.max_search_distance)) + else if(auto search_res_opt = map_ptr_->searchContainingFace( + robot_pos_, config_.max_search_distance)) { // update variables to new face auto search_res = *search_res_opt; - current_face = face = std::get<0>(search_res); + current_face_ = face = std::get<0>(search_res); vertices = std::get<1>(search_res); bary_coords = std::get<2>(search_res); - robot_pos = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); + robot_pos_ = mesh_map::linearCombineBarycentricCoords(vertices, bary_coords); } else { // no corresponding face has been found - return mbf_msgs::ExePathResult::OUT_OF_MAP; + return mbf_msgs::action::ExePath::Result::OUT_OF_MAP; } } - const lvr2::FaceHandle& face = current_face.unwrap(); - std::array handles = map_ptr->mesh_ptr->getVerticesOfFace(face); + const lvr2::FaceHandle& face = current_face_.unwrap(); + std::array handles = map_ptr_->mesh_ptr->getVerticesOfFace(face); // update to which position of the plan the robot is closest - const auto& opt_dir = map_ptr->directionAtPosition(vector_map, handles, bary_coords); + const auto& opt_dir = map_ptr_->directionAtPosition(vector_map_, handles, bary_coords); if (!opt_dir) { - DEBUG_CALL(map_ptr->publishDebugFace(face, mesh_map::color(0.3, 0.4, 0), "no_directions");) - ROS_ERROR_STREAM("Could not access vector field for the given face!"); - return mbf_msgs::ExePathResult::FAILURE; + DEBUG_CALL(map_ptr_->publishDebugFace(face, mesh_map::color(0.3, 0.4, 0), "no_directions");) + RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not access vector field for the given face!"); + return mbf_msgs::action::ExePath::Result::FAILURE; } mesh_map::Normal mesh_dir = opt_dir.get().normalized(); - float cost = map_ptr->costAtPosition(handles, bary_coords); + float cost = map_ptr_->costAtPosition(handles, bary_coords); const mesh_map::Normal& mesh_normal = poseToDirectionVector(pose, tf2::Vector3(0,0,1)); - std::array velocities = naiveControl(robot_pos, robot_dir, mesh_dir, mesh_normal, cost); - cmd_vel.twist.linear.x = std::min(config.max_lin_velocity, velocities[0] * config.lin_vel_factor); - cmd_vel.twist.angular.z = std::min(config.max_ang_velocity, velocities[1] * config.ang_vel_factor); - cmd_vel.header.stamp = ros::Time::now(); + std::array velocities = naiveControl(robot_pos_, robot_dir_, mesh_dir, mesh_normal, cost); + cmd_vel.twist.linear.x = std::min(config_.max_lin_velocity, velocities[0] * config_.lin_vel_factor); + cmd_vel.twist.angular.z = std::min(config_.max_ang_velocity, velocities[1] * config_.ang_vel_factor); + cmd_vel.header.stamp = node_->now(); - if (cancel_requested) + if (cancel_requested_) { - return mbf_msgs::ExePathResult::CANCELED; + return mbf_msgs::action::ExePath::Result::CANCELED; } - return mbf_msgs::ExePathResult::SUCCESS; + return mbf_msgs::action::ExePath::Result::SUCCESS; } bool MeshController::isGoalReached(double dist_tolerance, double angle_tolerance) { - float goal_distance = (goal_pos - robot_pos).length(); - float angle = acos(goal_dir.dot(robot_dir)); + float goal_distance = (goal_pos_ - robot_pos_).length(); + float angle = acos(goal_dir_.dot(robot_dir_)); return goal_distance <= static_cast(dist_tolerance) && angle <= static_cast(angle_tolerance); } -bool MeshController::setPlan(const std::vector& plan) +bool MeshController::setPlan(const std::vector& plan) { // copy vector field // TODO just use vector field without copying - vector_map = map_ptr->getVectorMap(); - DEBUG_CALL(map_ptr->publishDebugPoint(poseToPositionVector(plan.front()), mesh_map::color(0, 1, 0), "plan_start");) - DEBUG_CALL(map_ptr->publishDebugPoint(poseToPositionVector(plan.back()), mesh_map::color(1, 0, 0), "plan_goal");) - current_plan = plan; - goal_pos = poseToPositionVector(current_plan.back()); - goal_dir = poseToDirectionVector(current_plan.back()); + vector_map_ = map_ptr_->getVectorMap(); + DEBUG_CALL(map_ptr_->publishDebugPoint(poseToPositionVector(plan.front()), mesh_map::color(0, 1, 0), "plan_start");) + DEBUG_CALL(map_ptr_->publishDebugPoint(poseToPositionVector(plan.back()), mesh_map::color(1, 0, 0), "plan_goal");) + current_plan_ = plan; + goal_pos_ = poseToPositionVector(current_plan_.back()); + goal_dir_ = poseToDirectionVector(current_plan_.back()); // reset current and ahead face - cancel_requested = false; - current_face = lvr2::OptionalFaceHandle(); + cancel_requested_ = false; + current_face_ = lvr2::OptionalFaceHandle(); return true; } bool MeshController::cancel() { - ROS_INFO_STREAM("The MeshController has been requested to cancel!"); - cancel_requested = true; + RCLCPP_INFO_STREAM(node_->get_logger(), "The MeshController has been requested to cancel!"); + cancel_requested_ = true; return true; } -mesh_map::Normal MeshController::poseToDirectionVector(const geometry_msgs::PoseStamped& pose, const tf2::Vector3& axis) +mesh_map::Normal MeshController::poseToDirectionVector(const geometry_msgs::msg::PoseStamped& pose, const tf2::Vector3& axis) { tf2::Stamped transform; fromMsg(pose, transform); @@ -209,7 +208,7 @@ mesh_map::Normal MeshController::poseToDirectionVector(const geometry_msgs::Pose return mesh_map::Normal(v.x(), v.y(), v.z()); } -mesh_map::Vector MeshController::poseToPositionVector(const geometry_msgs::PoseStamped& pose) +mesh_map::Vector MeshController::poseToPositionVector(const geometry_msgs::msg::PoseStamped& pose) { return mesh_map::Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z); } @@ -229,31 +228,130 @@ std::array MeshController::naiveControl( float phi = acos(mesh_dir.dot(robot_dir)); float sign_phi = mesh_dir.cross(robot_dir).dot(mesh_normal); // debug output angle between supposed and current angle - DEBUG_CALL(std_msgs::Float32 angle32; angle32.data = phi * 180 / M_PI; angle_pub.publish(angle32);) + DEBUG_CALL(example_interfaces::msg::Float32 angle32; angle32.data = phi * 180 / M_PI; angle_pub_->publish(angle32);) - float angular_velocity = copysignf(phi * config.max_ang_velocity / M_PI, -sign_phi); - const float max_angle = config.max_angle * M_PI / 180.0; - const float max_linear = config.max_lin_velocity; + float angular_velocity = copysignf(phi * config_.max_ang_velocity / M_PI, -sign_phi); + const float max_angle = config_.max_angle * M_PI / 180.0; + const float max_linear = config_.max_lin_velocity; float linear_velocity = phi <= max_angle ? max_linear - (phi * max_linear / max_angle) : 0.0; return { linear_velocity, angular_velocity }; } -void MeshController::reconfigureCallback(mesh_controller::MeshControllerConfig& cfg, uint32_t level) +rcl_interfaces::msg::SetParametersResult MeshController::reconfigureCallback(std::vector parameters) { - config = cfg; + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + if (parameter.get_name() == name_ + ".max_lin_velocity") { + config_.max_lin_velocity = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".max_ang_velocity") { + config_.max_ang_velocity= parameter.as_double(); + } else if (parameter.get_name() == name_ + ".arrival_fading") { + config_.arrival_fading = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".ang_vel_factor") { + config_.ang_vel_factor = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".lin_vel_factor") { + config_.lin_vel_factor = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".max_angle") { + config_.max_angle = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".max_search_radius") { + config_.max_search_radius = parameter.as_double(); + } else if (parameter.get_name() == name_ + ".max_search_distance") { + config_.max_search_distance = parameter.as_double(); + } + } + + result.successful = true; + return result; } -bool MeshController::initialize(const std::string& plugin_name, const boost::shared_ptr& tf_ptr, - const boost::shared_ptr& mesh_map_ptr) +bool MeshController::initialize(const std::string& plugin_name, + const std::shared_ptr& tf_ptr, + const std::shared_ptr& mesh_map_ptr, + const rclcpp::Node::SharedPtr& node) { - ros::NodeHandle private_nh("~/" + plugin_name); - map_ptr = mesh_map_ptr; - reconfigure_server_ptr = boost::shared_ptr>( - new dynamic_reconfigure::Server(private_nh)); - - config_callback = boost::bind(&MeshController::reconfigureCallback, this, _1, _2); - reconfigure_server_ptr->setCallback(config_callback); - angle_pub = private_nh.advertise("current_angle", 1); + node_ = node; + map_ptr_ = mesh_map_ptr; + name_ = plugin_name; + + angle_pub_ = node_->create_publisher("~/current_angle", rclcpp::QoS(1).transient_local()); + + { // cost max_lin_velocity + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Defines the maximum linear velocity"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 5.0; + descriptor.floating_point_range.push_back(range); + config_.max_lin_velocity = node->declare_parameter(name_ + ".max_lin_velocity", config_.max_lin_velocity); + } + { // cost max_ang_velocity + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Defines the maximum angular velocity"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 2.0; + descriptor.floating_point_range.push_back(range); + config_.max_ang_velocity = node->declare_parameter(name_ + ".max_ang_velocity", config_.max_ang_velocity); + } + { // cost arrival_fading + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Distance to goal position where the robot starts to fade down the linear velocity"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 5.0; + descriptor.floating_point_range.push_back(range); + config_.arrival_fading = node->declare_parameter(name_ + ".arrival_fading", config_.arrival_fading); + } + { // cost ang_vel_factor + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Factor for angular velocity"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.1; + range.to_value = 10.0; + descriptor.floating_point_range.push_back(range); + config_.ang_vel_factor = node->declare_parameter(name_ + ".ang_vel_factor", config_.ang_vel_factor); + } + { // cost lin_vel_factor + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Factor for linear velocity"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.1; + range.to_value = 10.0; + descriptor.floating_point_range.push_back(range); + config_.lin_vel_factor = node->declare_parameter(name_ + ".lin_vel_factor", config_.lin_vel_factor); + } + { // cost max_angle + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "The maximum angle for the linear velocity function"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 1.0; + range.to_value = 180.0; + descriptor.floating_point_range.push_back(range); + config_.max_angle = node->declare_parameter(name_ + ".max_angle", config_.max_angle); + } + { // cost max_search_radius + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "The maximum radius in which to search for a consecutive neighbour face"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.01; + range.to_value = 2.0; + descriptor.floating_point_range.push_back(range); + config_.max_search_radius = node->declare_parameter(name_ + ".max_search_radius", config_.max_search_radius); + } + { // cost max_search_distance + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "The maximum distance from the surface which is accepted for projection"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.01; + range.to_value = 2.0; + descriptor.floating_point_range.push_back(range); + config_.max_search_distance = node->declare_parameter(name_ + ".max_search_distance", config_.max_search_distance); + } + + reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind( + &MeshController::reconfigureCallback, this, std::placeholders::_1)); + return true; } } /* namespace mesh_controller */