.. index:: navigate_to_pose

.. _skill-navigate_to_pose:

Skill ``navigate_to_pose``
-----------------------------------------------------------


- **Default path**: ``/skill/navigate_to_pose``
- **Datatype**: :action:`navigation_skills/action/NavigateToPose`
- **Definition source**: ``package.xml`` in `ros4hri/navigation_skills <https://github.com/ros4hri/navigation_skills>`_

Make the robot autonomously navigate toward the desired pose.

See :ref:`goal-navigation` for details.




Input parameters
~~~~~~~~~~~~~~~~



- **pose** ``geometry_msgs/PoseStamped``, *required*

  
  The coordinates of the pose that the robot must reach.





- **behavior_tree** ``string``

  
  The name or the full path to an XML file with a behavior tree
  defining the logic that the robot must use while autonomously navigate
  towards its goal.





- **meta.priority** ``integer``, default: ``128``

  
  Between 0 and 255. Higher value means that this skill invokation will have
  higher priority.








Output fields
~~~~~~~~~~~~~



- **navigation_result** ``empty``

  
  The result of the navigation action. Information about the navigation
  execution are available in the action feedback.








Quick snippets
~~~~~~~~~~~~~~


.. tabs:: 

  .. tab:: Command-line

    Call the skill from the command-line
    

    .. code-block:: sh
    
        $ ros2 action send_goal /skill/navigate_to_pose navigation_skills/action/NavigateToPose # then press Tab to complete the message prototype


  .. tab:: ROS 2 action (Python)

    Call the action from a Python script:


    .. code-block:: python
    
        #!/usr/bin/env python

        import rclpy
        from rclpy.action import ActionClient
        from rclpy.node import Node

        from navigation_skills.action import 

        class NavigatetoposeActionClient(Node):

            def __init__(self):
                super().__init__('navigate_to_pose_client')
                self._action_client = ActionClient(self, , '/skill/navigate_to_pose')

            def send_goal(self, a, b):
                goal_msg = .Goal()

                # TODO: adapt to the action's parameters

                # check the NavigateToPoseGoal message
                # definition for the possible goal parameters

                # goal_msg.a = a
                # goal_msg.b = b

                self._action_client.wait_for_server()

                return self._action_client.send_goal_async(goal_msg)

        if __name__ == '__main__':
            rclpy.init(args=args)

            action_client = ActionClient()

            # TODO: adapt to your action's parameters
            future = action_client.send_goal(a, b)

            rclpy.spin_until_future_complete(action_client, future)

            rclpy.shutdown()



  
  .. tab:: ROS 2 action (C++)

    Call the action from a C++ program:

    .. code-block:: cpp

        #include <functional>
        #include <future>
        #include <memory>
        #include <string>
        #include <sstream>
        #include <chrono>

        #include "navigation_skills/action/.hpp"

        #include "rclcpp/rclcpp.hpp"
        #include "rclcpp_action/rclcpp_hpp"
        #include "rclcpp_components/register_node_macro.hpp"

        using namespace std::chrono_literals;
        using namespace std;

        class NavigatetoposeActionClient : public rclcpp::Node
        {
        public:
          using  = navigation_skills::action::;
          using GoalHandle = rclcpp_action::ClientGoalHandle<>;

          explicit NavigatetoposeActionClient(const rclcpp::NodeOptions & options)
          : Node("navigate_to_pose_action_client", options)
          {
            this->client_ptr_ = rclcpp_action::create_client<>(
                    this,
                    "");

            this->timer_ = this->create_wall_timer(
                    500ms,
                    bind(&NavigatetoposeActionClient::send_goal, this));
          }

          void send_goal()
          {
            using namespace std::placeholders;

            this->timer_->cancel();

            if (!this->client_ptr_->wait_for_action_server()) {
              RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
              rclcpp::shutdown();
            }

            auto goal_msg = ::Goal();
  

            // check the NavigateToPoseGoal message
            // definition for the possible goal parameters

            // goal_msg.... = ...;

            RCLCPP_INFO(this->get_logger(), "Sending goal");

            auto send_goal_options = rclcpp_action::Client<>::SendGoalOptions();

            send_goal_options.goal_response_callback =
                bind(&NavigatetoposeActionClient::goal_response_callback, this, _1);

            send_goal_options.feedback_callback =
                bind(&NavigatetoposeActionClient::feedback_callback, this, _1, _2);

            send_goal_options.result_callback =
                bind(&NavigatetoposeActionClient::result_callback, this, _1);


            this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
          }

        private:
          rclcpp_action::Client<>::SharedPtr client_ptr_;
          rclcpp::TimerBase::SharedPtr timer_;

          void goal_response_callback(const GoalHandle::SharedPtr & goal_handle)
          {
            if (!goal_handle) {
              RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
            } else {
              RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
            }
          }

          void feedback_callback(
              GoalHandle::SharedPtr,
              const shared_ptr<const ::Feedback> feedback)
          {
            stringstream ss;
            ss << "Next number in sequence received: ";
            for (auto number : feedback->partial_sequence) {
              ss << number << " ";
            }
            RCLCPP_INFO(this->get_logger(), ss.str().c_str());
          }

          void result_callback(const GoalHandle::WrappedResult & result)
          {
            switch (result.code) {
              case rclcpp_action::ResultCode::SUCCEEDED:
                  break;
              case rclcpp_action::ResultCode::ABORTED:
                  RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
                  return;
              case rclcpp_action::ResultCode::CANCELED:
                  RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
                  return;
              default:
                  RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                  return;
            }
            stringstream ss;
            ss << "Result received: ";
            for (auto number : result.result->sequence) {
              ss << number << " ";
            }
            RCLCPP_INFO(this->get_logger(), ss.str().c_str());
            rclcpp::shutdown();
          }
        };  // class NavigatetoposeActionClient


        RCLCPP_COMPONENTS_REGISTER_NODE(NavigatetoposeActionClient)

  .. tab:: QML

    You can call this skill from QML using the following code snippet. See :ref:`ros_qml_plugin` to learn more.

    .. code-block:: qml

        import Ros 2.0
        
        // ...
        NavigateToPoseSkill {
            id: mySkill

            onResult: {
                console.log("Skill result: " + result);
            }
            onFeedback: {
                console.log("Skill feedback: " + feedback);
            }

        }

        // Call the skill
        mySkill.navigate_to_pose();


See also
~~~~~~~~


* List of other :ref:`navigation_skills` skills