diff --git a/docs_outdoornav_user_manual/api/api_endpoints/all_other_endpoints.mdx b/docs_outdoornav_user_manual/api/api_endpoints/all_other_endpoints.mdx new file mode 100644 index 00000000..50887206 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/all_other_endpoints.mdx @@ -0,0 +1,190 @@ +--- +title: All Other Endpoints +sidebar_label: All Other Endpoints +sidebar_position: 6 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +## Topics + +
+localization/datum + +The lat/lon coordinate of the datum. This value is sotred in persistent memory and loaded at runtime. + +- Topic Name: **localization/datum** + +- Topic Type: [sensor_msgs/msg/NavSatFix](https://github.com/ros2/common_interfaces/blob/jazzy/sensor_msgs/msg/NavSatFix.msg) + +- Topic QoS: + + + + +```python +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import NavSatFix + + +class Datum(Node): + + def __init__(self): + super().__init__('datum_sub') + self.datum_sub = self.create_subscription( + NavSatFix, 'localization/datum', self.datum_cb, 10) + self.datum_sub # prevent unused variable warning + + def datum_cb(self, msg): + self.last_datum_msg = msg + +if __name__ == '__main__': + rclpy.init(args=args) + datum_sub = Datum() + rclpy.spin(datum_sub) + datum_sub.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +class Datum : public rclcpp::Node +{ +public: + Datum() + : Node("datum_sub") + { + auto datum_cb = + [this](sensor_msgs::msg::NavSatFix::UniquePtr msg) -> void { + last_datum_msg_ = msg; + }; + + datum_sub_ = this->create_subscription("localization/datum", 10, datum_cb); + } + +private: + rclcpp::Subscription::SharedPtr datum_sub_; + sensor_msgs::msg::NavSatFix::UniquePtr last_datum_msg_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +``` + + + +
+ + +## Services + +
+control_selection/set_mode + +Switch the control mode of the platform. Options are: ('AUTONOMY', 'MANUAL', 'NEUTRAL') + +- Service Name: **control_selection/set_mode** + +- Service Type: [clearpath_control_msgs/srv/SetControlMode](pathname:///api/html/clearpath_control_msgs/srv/SetControlMode.html) + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_control_msgs.srv import SetControlMode, +from clearpath_control_msgs.msg import ControlMode + + +class SetControlMode(Node): + + def __init__(self): + super().__init__('set_control_mode_client') + self._srv_client = self.create_client(SetControlMode, 'control_selection/set_mode') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SetControlMode.Request() + + def send_request(self, mode): + # set to manual mode + self.req.mode.mode = mode + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + set_control_mode_client = SetControlMode() + response = set_control_mode_client.send_request(ControlMode.MANUAL) + set_control_mode_client.get_logger().info('Control mode set to MANUAL mode.')) + set_control_mode_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_control_msgs/srv/set_control_mode.hpp" +#include "clearpath_control_msgs/msg/control_mode.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("set_control_mode_client"); + rclcpp::Client::SharedPtr client = + node->create_client("control_selection/set_mode"); + + auto request = std::make_shared(); + request-> mode.mode = clearpath_control_msgs::ControlMode::MANUAL; + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Control mode set to MANUAL mode"); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service control_selection/set_mode"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + +
diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/_category_.json b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/_category_.json new file mode 100644 index 00000000..a6e07f22 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Autonomy", + "position": 1 +} diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/execute_task.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/execute_task.mdx new file mode 100644 index 00000000..2dfc24f6 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/execute_task.mdx @@ -0,0 +1,296 @@ +--- +title: Execute Task +sidebar_label: Execute Task +sidebar_position: 11 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +## Service + +Execute a task as a service. + +- Service Name: **autonomy/task/execute_as_srv** + +- Service Type: [clearpath_task_msgs/srv/ExecuteTask](pathname:///api/html/clearpath_task_msgs/srv/ExecuteTask.html) + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_task_msgs.srv import ExecuteTask + + +class ExecuteTask(Node): + + def __init__(self): + super().__init__('execute_task_client') + self._srv_client = self.create_client(ExecuteTask, 'autonomy/task/execute_as_srv') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = ExecuteTask.Request() + + def send_request(self): + # TODO + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + execute_task_client = PauseAutonomy() + response = execute_task_client.send_request() + if response.success: + execute_task_client.get_logger().info('Executing task: ')) + else: + execute_task_client.get_logger().error('Failed to execute task: ') + + execute_task_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_task_msgs/srv/execute_task.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("execute_task_client"); + rclcpp::Client::SharedPtr client = + node->create_client("autonomy/taskexecute_as_srv"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Executing task: ", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to execute task: "); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/task/execute_as_srv"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + + + +## Action + +Execute a task as an action. + +- Action Name: **autonomy/task/execute_as_action** + +- Action Type: [clearpath_task_msgs/action/ExecuteTask](pathname:///api/html/clearpath_task_msgs/action/ExecuteTask.html) + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_task_msgs.action import ExecuteTask + + +class ExecuteTask(Node): + + def __init__(self): + super().__init__('execute_task_action_client') + self._action_client = ActionClient(self, ExecuteTask, 'autonomy/task/execute_as_action') + + def send_goal(self): + goal_msg = ExecuteTask.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = ExecuteTask() + action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_task_msgs/action/execute_task.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace execute_task_action_client +{ +class ExecuteTaskActionClient : public rclcpp::Node +{ +public: + using ExecuteTask = action_tutorials_interfaces::action::ExecuteTask; + using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle; + + explicit ExecuteGoToActionClient(const rclcpp::NodeOptions & options) + : Node("execute_task_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/task/execute_as_action"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ExecuteTaskActionClient::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 = ExecuteTask::Goal(); + // TODO: update goal values + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ExecuteTaskActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteTaskActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&ExecuteTaskActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleExecuteTask::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleExecuteTask::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; +// } +// std::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 ExecuteTaskActionClient + +} // namespace execute_task_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(goto_action_client::ExecuteTaskActionClient) +``` + + + \ No newline at end of file diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto.mdx new file mode 100644 index 00000000..51b11d8f --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto.mdx @@ -0,0 +1,193 @@ +--- +title: Execute GoTo +sidebar_label: Execute GoTo +sidebar_position: 1 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Send the platform to a location on the map. + +- Action Name: **autonomy/goto** + +- Action Type: [clearpath_navigation_msgs/action/ExecuteGoTo](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteGoTo.html) + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import ExecuteGoTo + + +class ExecuteGoToActionClient(Node): + + def __init__(self): + super().__init__('goto_action_client') + self._action_client = ActionClient(self, ExecuteGoTo, 'autonomy/goto') + + def send_goal(self, order): + goal_msg = ExecuteGoTo.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = ExectuteGoToActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_navigation_msgs/action/execute_go_to.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace goto_action_client +{ +class ExecuteGoToActionClient : public rclcpp::Node +{ +public: + using ExecuteGoTo = action_tutorials_interfaces::action::ExecuteGoTo; + using GoalHandleExecuteGoTo = rclcpp_action::ClientGoalHandle; + + explicit ExecuteGoToActionClient(const rclcpp::NodeOptions & options) + : Node("goto_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/goto"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ExecuteGoToActionClient::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 = ExecuteGoTo::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ExecuteGoToActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteGoToActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&ExecuteGoToActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleExecuteGoTo::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleExecuteGoTo::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; +// } +// std::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 ExecuteGoToActionClient + +} // namespace goto_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(goto_action_client::ExecuteGoToActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto_poi.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto_poi.mdx new file mode 100644 index 00000000..4c291c6c --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto_poi.mdx @@ -0,0 +1,194 @@ +--- +title: Execute GoTo POI +sidebar_label: Execute GoTo POI +sidebar_position: 2 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Send the platform to a point of interest (POI) on the map. + +Action Name: **autonomy/goto_poi** + +Action Type: [clearpath_navigation_msgs/action/ExecuteGoToPOI](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteGoToPOI.html) + + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import ExecuteGoToPOI + + +class ExecuteGoToPOIActionClient(Node): + + def __init__(self): + super().__init__('goto_poi__action_client') + self._action_client = ActionClient(self, ExecuteGoToPOI, 'autonomy/goto_poi') + + def send_goal(self, order): + goal_msg = ExecuteGoToPOI.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = ExectuteGoToPOIActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_navigation_msgs/action/execute_go_to_poi.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace goto_poi_action_client +{ +class ExecuteGoToPOIActionClient : public rclcpp::Node +{ +public: + using ExecuteGoToPOI = action_tutorials_interfaces::action::ExecuteGoToPOI; + using GoalHandleExecuteGoToPOI = rclcpp_action::ClientGoalHandle; + + explicit ExecuteGoToPOIActionClient(const rclcpp::NodeOptions & options) + : Node("goto_poi_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/goto_poi"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ExecuteGoToPOIActionClient::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 = ExecuteGoToPOI::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ExecuteGoToPOIActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteGoToPOIActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&ExecuteGoToPOIActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleExecuteGoToPOI::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleExecuteGoToPOI::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; +// } +// std::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 ExecuteGoToPOIActionClient + +} // namespace goto_poi_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(goto_poi_action_client::ExecuteGoToPOIActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_dock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_dock.mdx new file mode 100644 index 00000000..c223aa1a --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_dock.mdx @@ -0,0 +1,194 @@ +--- +title: Dock (Local) +sidebar_label: Dock (Local) +sidebar_position: 6 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Dock the robot. The dock target must be visible for this operation to succeed. + +Action Name: **autonomy/dock/local** + +Action Type: [clearpath_dock_msgs/action/Dock](pathname:///api/html/clearpath_dock_msgs/action/Dock.html) + + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import Dock + + +class DockActionClient(Node): + + def __init__(self): + super().__init__('local_dock_action_client') + self._action_client = ActionClient(self, Dock, 'autonomy/dock/local') + + def send_goal(self, order): + goal_msg = Dock.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = DockActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_dock_msgs/action/dock.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace local_dock_action_client +{ +class DockActionClient : public rclcpp::Node +{ +public: + using Dock = action_tutorials_interfaces::action::Dock; + using GoalHandleDock = rclcpp_action::ClientGoalHandle; + + explicit DockActionClient(const rclcpp::NodeOptions & options) + : Node("local_dock_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/dock/local"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&DockActionClient::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 = Dock::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&DockActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&DockActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&DockActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleDock::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleDock::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; +// } +// std::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 DockActionClient + +} // namespace local_dock_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(local_dock_action_client::DockActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_undock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_undock.mdx new file mode 100644 index 00000000..981b8dd5 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/local_undock.mdx @@ -0,0 +1,194 @@ +--- +title: Undock (Local) +sidebar_label: Undock (Local) +sidebar_position: 7 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Undock the robot. The shold ideally be docked when this is run for successfull operation. + +Action Name: **autonomy/undock/local** + +Action Type: [clearpath_dock_msgs/action/Undock](pathname:///api/html/clearpath_dock_msgs/action/Undock.html) + + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import Undock + + +class UndockActionClient(Node): + + def __init__(self): + super().__init__('local_undock_action_client') + self._action_client = ActionClient(self, Undock, 'autonomy/dock/local') + + def send_goal(self, order): + goal_msg = Undock.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = UndockActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_dock_msgs/action/undock.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace local_undock_action_client +{ +class UndockActionClient : public rclcpp::Node +{ +public: + using Undock = action_tutorials_interfaces::action::Undock; + using GoalHandleUndock = rclcpp_action::ClientGoalHandle; + + explicit UndockActionClient(const rclcpp::NodeOptions & options) + : Node("local_undock_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/dock/local"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&UndockActionClient::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 = Undock::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&UndockActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&UndockActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&UndockActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleUndock::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleUndock::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; +// } +// std::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 UndockActionClient + +} // namespace local_undock_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(local_undock_action_client::UndockActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/map_dock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/map_dock.mdx new file mode 100644 index 00000000..617692b3 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/map_dock.mdx @@ -0,0 +1,195 @@ +--- +title: Dock (Map) +sidebar_label: Dock (Map) +sidebar_position: 5 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + + +Send the platform to a dock that is located on the map. + +Action Name: **autonomy/dock/map* + +Action Type: [clearpath_dock_msgs/action/MapDock](pathname:///api/html/clearpath_dock_msgs/action/MapDock.html) + + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import MapDock + + +class MapDockActionClient(Node): + + def __init__(self): + super().__init__('map_dock_action_client') + self._action_client = ActionClient(self, MapDock, 'autonomy/dock/map') + + def send_goal(self, order): + goal_msg = MapDock.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = MapDockActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_dock_msgs/action/map_dock.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace map_dock_action_client +{ +class MapDockActionClient : public rclcpp::Node +{ +public: + using MapDock = action_tutorials_interfaces::action::MapDock; + using GoalHandleMapDock = rclcpp_action::ClientGoalHandle; + + explicit MapDockActionClient(const rclcpp::NodeOptions & options) + : Node("map_dock_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/dock/map"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&MapDockActionClient::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 = MapDock::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&MapDockActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&MapDockActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&MapDockActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleMapDock::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleMapDock::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; +// } +// std::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 MapDockActionClient + +} // namespace map_dock_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(map_dock_action_client::MapDockActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission.mdx new file mode 100644 index 00000000..cddfed42 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission.mdx @@ -0,0 +1,194 @@ +--- +title: Execute Mission +sidebar_label: Execute Mission +sidebar_position: 3 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + + +Send the platform on an autonomous mission to various locations around the map. + +- Action Name: **autonomy/mission** + +- Action Type: [clearpath_navigation_msgs/action/ExecuteMission](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteMission.html) + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import ExecuteMission + + +class ExecuteMissionActionClient(Node): + + def __init__(self): + super().__init__('mission_action_client') + self._action_client = ActionClient(self, ExecuteMission, 'autonomy/mission') + + def send_goal(self, order): + goal_msg = ExecuteMission.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + # def get_result_callback(self, future): + # result = future.result().result + # self.get_logger().info('Result: {0}'.format(result.sequence)) + # rclpy.shutdown() + + # def feedback_callback(self, feedback_msg): + # feedback = feedback_msg.feedback + # self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = ExecuteMissionActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_navigation_msgs/action/execute_mission.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace mission_action_client +{ +class ExecuteMissionActionClient : public rclcpp::Node +{ +public: + using ExecuteMission = action_tutorials_interfaces::action::ExecuteMission; + using GoalHandleExecuteMission = rclcpp_action::ClientGoalHandle; + + explicit ExecuteMissionActionClient(const rclcpp::NodeOptions & options) + : Node("mission_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/mission"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ExecuteMissionActionClient::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 = ExecuteMission::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ExecuteMissionActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteMissionActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&ExecuteMissionActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleExecuteMission::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleExecuteMission::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; +// } +// std::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 ExecuteMissionActionClient + +} // namespace mission_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(mission_action_client::ExecuteMissionActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission_from_goal.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission_from_goal.mdx new file mode 100644 index 00000000..d60e7983 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/mission_from_goal.mdx @@ -0,0 +1,195 @@ +--- +title: Execute Mission from Goal +sidebar_label: Execute Mission from Goal +sidebar_position: 4 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + + +Send the platform on an autonomous mission around the map, starting from a specific goal. + +Action Name: **autonomy/mission_from_goal** + +Action Type: [clearpath_navigation_msgs/action/ExecuteMissionFromGoal](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteMissionFromGoal.html) + + + + + +```python +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from clearpath_navigation_msgs.action import ExecuteMissionFromGoal + + +class ExecuteMissionFromGoalActionClient(Node): + + def __init__(self): + super().__init__('mission_from_goal_action_client') + self._action_client = ActionClient(self, ExecuteMissionFromGoal, 'autonomy/mission_from_goal') + + def send_goal(self, order): + goal_msg = ExecuteMissionFronGoal.Goal() + + # TODO: populate goal message + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + # def get_result_callback(self, future): + # result = future.result().result + # self.get_logger().info('Result: {0}'.format(result.sequence)) + # rclpy.shutdown() + + # def feedback_callback(self, feedback_msg): + # feedback = feedback_msg.feedback + # self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +if __name__ == '__main__': + rclpy.init(args=args) + action_client = ExecuteMissionFromGoalActionClient() + # action_client.send_goal() + rclpy.spin(action_client) +``` + + + +```cpp +#include +#include +#include +#include +#include + +#include "clearpath_navigation_msgs/action/execute_mission_from_goal.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace mission_from_goal_action_client +{ +class ExecuteMissionFromGoalActionClient : public rclcpp::Node +{ +public: + using ExecuteMissionFromGoal = action_tutorials_interfaces::action::ExecuteMissionFromGoal; + using GoalHandleExecuteMissionFromGoal = rclcpp_action::ClientGoalHandle; + + explicit ExecuteMissionFromGoalActionClient(const rclcpp::NodeOptions & options) + : Node("mission_from_goal_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client( + this, "autonomy/mission_from_goal"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ExecuteMissionFromGoalActionClient::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 = ExecuteMissionFromGoal::Goal(); + // TODO: update goal values + // goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&ExecuteMissionFromGoalActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&ExecuteMissionFromGoalActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&ExecuteMissionFromGoalActionClient::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(std::shared_future future) + { + auto goal_handle = future.get(); + 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"); + } + } + +// TODO: update with action feedback +// void feedback_callback( +// GoalHandleExecuteMissionFromGoal::SharedPtr, +// const std::shared_ptr feedback) +// { +// std::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()); +// } + +// TODO: update with action result +// void result_callback(const GoalHandleExecuteMissionFromGoal::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; +// } +// std::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 ExecuteMissionFromGoalActionClient + +} // namespace mission_from_goal_action_client + +RCLCPP_COMPONENTS_REGISTER_NODE(mission_from_goal_action_client::ExecuteMissionFromGoalActionClient) +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/pause_autonomy.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/pause_autonomy.mdx new file mode 100644 index 00000000..0a6c6b15 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/pause_autonomy.mdx @@ -0,0 +1,109 @@ +--- +title: Pause Autonomy +sidebar_label: Pause Autonomy +sidebar_position: 9 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Pause the currently running autonomy action. + +- Service Name: **autonomy/pause** + +- Service Type: [std_srvs/srv/SetBool](https://docs.ros2.org/foxy/api/std_srvs/srv/SetBool.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import SetBool + + +class PauseAutonomy(Node): + + def __init__(self): + super().__init__('pause_autonomy_client') + self._srv_client = self.create_client(Trigger, 'autonomy/pause') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SetBool.Request() + + def send_request(self): + self.req.data = True + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + pause_autonomy_client = PauseAutonomy() + response = pause_autonomy_client.send_request() + if response.success: + pause_autonomy_client.get_logger().info('Autonomy paused!')) + else: + pause_autonomy_client.get_logger().error('Autonomy failed to pause!') + + pause_autonomy_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("pause_autonomy_client"); + rclcpp::Client::SharedPtr client = + node->create_client("autonomy/pause"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy paused!", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy failed to pause!"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/pause"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/resume_autonomy.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/resume_autonomy.mdx new file mode 100644 index 00000000..906eeba7 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/resume_autonomy.mdx @@ -0,0 +1,110 @@ +--- +title: Resume Autonomy +sidebar_label: Resume Autonomy +sidebar_position: 10 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Resume autonomy from a previouly paused state. + +- Service Name: **autonomy/resume** + +- Service Type: [std_srvs/srv/SetBool](https://docs.ros2.org/foxy/api/std_srvs/srv/SetBool.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import SetBool + + +class ResumeAutonomy(Node): + + def __init__(self): + super().__init__('resume_autonomy_client') + self._srv_client = self.create_client(Trigger, 'autonomy/resume') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SetBool.Request() + + def send_request(self): + self.req.data = True + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + resume_autonomy_client = ResumeAutonomy() + response = resume_autonomy_client.send_request() + if response.success: + resume_autonomy_client.get_logger().info('Autonomy resumed.')) + else: + resume_autonomy_client.get_logger().error('Autonomy failed to resume!') + + resume_autonomy_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("resume_autonomy_client"); + rclcpp::Client::SharedPtr client = + node->create_client("autonomy/resume"); + + auto request = std::make_shared(); + request.data = true; + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy resumed", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy failed to resume!"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/resume"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy/stop_autonomy.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/stop_autonomy.mdx new file mode 100644 index 00000000..7c0d275e --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/autonomy/stop_autonomy.mdx @@ -0,0 +1,108 @@ +--- +title: Stop Autonomy +sidebar_label: Stop Autonomy +sidebar_position: 8 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Stop (cancel) all autonomy related actions. + +- Service Name: **autonomy/stop** + +- Service Type: [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import Trigger + + +class StopAutonomy(Node): + + def __init__(self): + super().__init__('stop_autonomy_client') + self._srv_client = self.create_client(Trigger, 'autonomy/stop') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = Trigger.Request() + + def send_request(self): + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + stop_autonomy_client = StopAutonomy() + response = stop_autonomy_client.send_request() + if response.success: + stop_autonomy_client.get_logger().info('Autonomy stopped!')) + else: + stop_autonomy_client.get_logger().error('Autonomy failed to stop!') + + stop_autonomy_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("stop_autonomy_client"); + rclcpp::Client::SharedPtr client = + node->create_client("autonomy/stop"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy stopped!", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy failed to stop!"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/stop"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/autonomy_api.mdx b/docs_outdoornav_user_manual/api/api_endpoints/autonomy_api.mdx deleted file mode 100644 index f307167a..00000000 --- a/docs_outdoornav_user_manual/api/api_endpoints/autonomy_api.mdx +++ /dev/null @@ -1,120 +0,0 @@ ---- -title: Autonomy API Endpoints -sidebar_label: Autonomy API -sidebar_position: 2 -toc_min_heading_level: 2 -toc_max_heading_level: 4 ---- - -:::note - -All nodes, topics, and services are namespace prefixed with the robot serial number. - -If your serial number is `cpr-a300-00001`, then your namespace -will be `a300_00001`. This can be overwritten in the [robot.yaml](../../../../docs/ros/config/yaml/overview) file. - -::: - -  - -## Subscribers {#autonomy-subscribers} - -  - -| Topic | Message type | Description | QoS | -| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | :-------------------------------------------- | -| platform/cmd_vel_out | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Continuous platform level velocity output | [System Default](/docs/ros/api/overview#system-default) | -| platform/odom | [nav_msgs/msg/Odometry](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Odometry.msg) | Platform wheel odometry | [System Default](/docs/ros/api/overview#system-default) | -| joy_teleop/cmd_vel | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Velocity commands from joystick inputs | [System Default](/docs/ros/api/overview#system-default) | - -  - -## Publishers {#autonomy-publishers} - -  - -| Topic | Message type | Description | QoS | -| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | :-------------------------------------------- | -| autonomy/config | [clearpath_navigation_msgs/msg/AutonomyConfig](pathname:///api/html/clearpath_navigation_msgs/msg/AutonomyConfig.html) | Autonomy configuration | [System Default](/docs/ros/api/overview#system-default) | -| autonomy/initial_path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Initial Path computed by autonomy | [System Default](/docs/ros/api/overview#system-default) | -| autonomy/status | [clearpath_navigation_msgs/msg/AutonomyStatus](pathname:///api/html/clearpath_navigation_msgs/msg/AutonomyStatus.html) | Status of the autonomy | [System Default](/docs/ros/api/overview#system-default) | -| control_selection/current_mode | [clearpath_control_msgs/msg/ControlMode](pathname:///api/html/clearpath_control_msgs/msg/ControlMode.html) | Current control mode (NEUTRAL, MANUAL, AUTONOMY). | [System Default](/docs/ros/api/overview#system-default) | -| control_selection/control_state | [clearpath_control_msgs/msg/ControlSelectionState](pathname:///api/html/clearpath_control_msgs/msg/ControlSelectionState.html) | Complete state of control selection node. | [System Default](/docs/ros/api/overview#system-default) | -| docking/docking_server/path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Docking path | -| docking/undocking_server/path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Undocking path | -| goto/preview | [clearpath_navigation_msgs/msg/GoToPreview](pathname:///api/html/clearpath_navigation_msgs/msg/GoToPreview.html) | Preview points for a GoTo execution | [System Default](/docs/ros/api/overview#system-default) | -| localization/datum | [sensor_msgs/msg/NavSatFix](https://github.com/ros2/common_interfaces/blob/jazzy/sensor_msgs/msg/NavSatFix.msg) | Map origin (0, 0), specified as the datum | [System Default](/docs/ros/api/overview#system-default) | -| localization/fix | [sensor_msgs/msg/NavSatFix](https://github.com/ros2/common_interfaces/blob/jazzy/sensor_msgs/msg/NavSatFix.msg) | Platform lat/lon coordinates | [System Default](/docs/ros/api/overview#system-default) | -| localization/odom | [nav_msgs/msg/Odometry](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Odometry.msg) | Platform map coordinates | [System Default](/docs/ros/api/overview#system-default) | -| mission/preview | [clearpath_navigation_msgs/msg/MissionPreview](pathname:///api/html/clearpath_navigation_msgs/msg/MissionPreview.html) | Preview points for a mission execution | [System Default](/docs/ros/api/overview#system-default) | -| speed_limit | [clearpath_navigation_msgs/msg/GoToPreview](https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg) | Current navigation speed limit | [System Default](/docs/ros/api/overview#system-default) | -| ui/heartbeat | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Heartbeat of the UI | [System Default](/docs/ros/api/overview#system-default) | -| ui_teleop/cmd_vel | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Velocity commands fron the UI joystick | [System Default](/docs/ros/api/overview#system-default) | - -  - -## Services {#autonomy-services} - -  - -| Service | Service type | Description | -| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | -| autonomy/stop | [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) | Stop all autonomy executions | -| control_selection/set_mode | [clearpath_control_msgs/srv/SetControlMode](pathname:///api/html/clearpath_control_msgs/srv/SetControlMode.html) | Set the control mode | -| control_selection/pause | [std_srvs/srv/SetBool](https://docs.ros2.org/foxy/api/std_srvs/srv/SetBool.html) | Pause execution | -| control_selection/resume | [std_srvs/srv/SetBool](https://docs.ros2.org/foxy/api/std_srvs/srv/SetBool.html) | Resume execution | -| docking/dock_manager/add_dock | [clearpath_dock_msgs/srv/AddDock](pathname:///api/html/clearpath_dock_msgs/srv/AddDock.html) | Add a dock | -| docking/dock_manager/clear_data | [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) | Clear all dock data | -| docking/dock_manager/delete_dock | [clearpath_dock_msgs/srv/RemoveDock](pathname:///api/html/clearpath_dock_msgs/srv/RemoveDock.html) | Delete a dock | -| docking/dock_manager/export | [clearpath_dock_msgs/srv/ExportData](pathname:///api/html/clearpath_dock_msgs/srv/ExportData.html) | Export dock data | -| docking/dock_manager/get_database | [clearpath_dock_msgs/srv/GetDockDatabase](pathname:///api/html/clearpath_dock_msgs/srv/GetDockDatabase.html) | Returns the entire dock database | -| docking/dock_manager/get_dock | [clearpath_dock_msgs/srv/GetDock](pathname:///api/html/clearpath_dock_msgs/srv/GetDock.html) | Returns a docks info | -| docking/dock_manager/import | [clearpath_dock_msgs/srv/ImportData](pathname:///api/html/clearpath_dock_msgs/srv/ImportData.html) | Import a dock | -| docking/dock_manager/update_dock | [clearpath_dock_msgs/srv/UpdateDock](pathname:///api/html/clearpath_dock_msgs/srv/UpdateDock.html) | Update a docks information | -| docking/dock_localizer/add_dock_current_pose| [clearpath_dock_msgs/srv/AddDockCurrentPose](pathname:///api/html/clearpath_dock_msgs/srv/AddDockCurrentPose.html) | Add a dock with the current pose | -| docking/dock_localizer/get_dock_poses| [clearpath_dock_msgs/srv/GetDockPoses](pathname:///api/html/clearpath_dock_msgs/srv/GetDockPoses.html) | Return the dock and predock poses of a specific dock | -| docking/dock_localizer/survey_dock| [clearpath_dock_msgs/srv/SurveyDock](pathname:///api/html/clearpath_dock_msgs/srv/SurveyDock.html) | Survey the docks position | -| execute_task | [clearpath_task_msgs/srv/ExecuteTask](pathname:///api/html/clearpath_task_msgs/srv/ExecuteTask.html) | Run a task as a service | -| localization/lat_lon_to_xy | [clearpath_localization_msgs/srv/ConvertLatLonToCartesian](pathname:///api/html/clearpath_localization_msgs/srv/ConvertLatLonToCartesian.html) | Convert lat/lon condinate to map XY coordinate | -| localization/lat_lon_to_xy_array | [clearpath_localization_msgs/srv/ConvertLatLonToCartesianArray](pathname:///api/html/clearpath_localization_msgs/srv/ConvertLatLonToCartesianArray.html) | Convert a set of lat/lon coordinates to map XY coordinates | -| localization/set_datum | [clearpath_localization_msgs/srv/SetDatum](pathname:///api/html/clearpath_localization_msgs/srv/SetDatum.html) | Set the datum | -| localization/xy_to_lat_lon | [clearpath_localization_msgs/srv/ConvertCartesianToLatLon](pathname:///api/html/clearpath_localization_msgs/srv/ConvertCartesianToLatLon.html) | Convert map XY coordinate to lat/lon coordinate | -| localization/xy_to_lat_lon_array | [clearpath_localization_msgs/srv/ConvertCartesianToLatLonArray](pathname:///api/html/clearpath_localization_msgs/srv/ConvertCartesianToLatLonArray.html) | Convert a set of map XY coordinates to lat/lon coordinates | -| log_manager/delete_log | [clearpath_logger_msgs/srv/DeleteLog](pathname:///api/html/clearpath_logger_msgs/srv/DeleteLog.html) | Delete a secific log | -| log_manager/start_recording | [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) | Start logging data | -| log_manager/stop_recording | [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) | Stop logging data | -| safety/watchdogs/add_communication_watchdog | [clearpath_safety_msgs/srv/AddCommunicationWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddCommunicationWatchdog.html) | Create a new Communication watchdog | -| safety/watchdogs/add_heartbeat_watchdog | [clearpath_safety_msgs/srv/AddHeartbeatWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddHeartbeatWatchdog.html) | Create a new Heartbeat watchdog | -| safety/watchdogs/add_inclination_watchdog | [clearpath_safety_msgs/srv/AddInclinationWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddInclinationWatchdog.html) | Create a new Inclination watchdog | -| safety/watchdogs/add_num_points_watchdog | [clearpath_safety_msgs/srv/AddNumPointsWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddNumPointsWatchdog.html) | Create a new NumPoints watchdog | -| safety/watchdogs/add_odom_covariance_watchdog | [clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.html) | Create a new OdomCovariance watchdog | -| safety/watchdogs/add_rtk_fix_watchdog | [clearpath_safety_msgs/srv/AddRtkFixWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddRtkFixWatchdog.html) | Create a new RTKFix watchdog | -| safety/watchdogs/add_topic_data_watchdog | [clearpath_safety_msgs/srv/AddTopicDataWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddTopicDataWatchdog.html) | Create a new TopicData watchdog | -| safety/watchdogs/add_trigger_watchdog | [clearpath_safety_msgs/srv/AddTriggerWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/AddTriggerWatchdog.html) | Create a new Trigger watchdog | -| safety/watchdogs/enable_all | [std_srvs/srv/SetBool](https://docs.ros2.org/foxy/api/std_srvs/srv/SetBool.html) | Enable/disable all watchdogs | -| safety/watchdogs/remove_watchdog | [clearpath_safety_msgs/srv/RemoveWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/RemoveWatchdog.html) | Remove/delete a watchdog | -| safety/watchdogs/update_communication_watchdog | [clearpath_safety_msgs/srv/UpdateCommunicationWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateCommunicationWatchdog.html) | Create a new Communication watchdog | -| safety/watchdogs/update_heartbeat_watchdog | [clearpath_safety_msgs/srv/UpdateHeartbeatWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateHeartbeatWatchdog.html) | Create a new Heartbeat watchdog | -| safety/watchdogs/update_inclination_watchdog | [clearpath_safety_msgs/srv/UpdateInclinationWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateInclinationWatchdog.html) | Create a new Inclination watchdog | -| safety/watchdogs/update_num_points_watchdog | [clearpath_safety_msgs/srv/UpdateNumPointsWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateNumPointsWatchdog.html) | Create a new NumPoints watchdog | -| safety/watchdogs/update_odom_covariance_watchdog | [clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog.html) | Create a new OdomCovariance watchdog | -| safety/watchdogs/update_rtk_fix_watchdog | [clearpath_safety_msgs/srv/UpdateRtkFixWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateRtkFixWatchdog.html) | Create a new RTKFix watchdog | -| safety/watchdogs/update_topic_data_watchdog | [clearpath_safety_msgs/srv/UpdateTopicDataWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateTopicDataWatchdog.html) | Create a new TopicData watchdog | -| safety/watchdogs/update_trigger_watchdog | [clearpath_safety_msgs/srv/UpdateTriggerWatchdog](pathname:///api/html/clearpath_safety_msgs/srv/UpdateTriggerWatchdog.html) | Create a new Trigger watchdog | - -  - -## Actions {#autonomy-actions} - -  - -| Action Name | Action type | Description | -| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | -| autonomy/network_mission | [clearpath_navigation_msgs/action/ExecuteNetworkMissionByUuid](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteNetworkMissionByUuid.html) | Execute an autonomous mission | -| autonomy/network_mission_from_goal | [clearpath_navigation_msgs/action/ExecuteNetworkMissionFromGoal](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteNetworkMissionFromGoal.html) | Execute an autonomous mission, starting from a specific goal | -| autonomy/network_goto | [clearpath_navigation_msgs/action/ExecuteNetworkGoTo](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteNetworkGoTo.html) | Send platform to location | -| autonomy/network_goto_poi | [clearpath_navigation_msgs/action/ExecuteNetworkGoToPOI](pathname:///api/html/clearpath_navigation_msgs/action/ExecuteNetworkGoToPOI.html) | Send platform to a point of interest | -| autonomy/local_dock | [clearpath_dock_msgs/action/Dock](pathname:///api/html/clearpath_dock_msgs/action/Dock.html) | Dock the platform (charge target must be visible by platform 2D lidar sensor) | -| autonomy/local_undock | [clearpath_dock_msgs/action/Undock](pathname:///api/html/clearpath_dock_msgs/action/Undock.html) | Undock the platform (charge target must be visible by the 2D lidar sensor) | -| autonomy/network_dock | [clearpath_dock_msgs/action/NetworkDock](pathname:///api/html/clearpath_dock_msgs/action/NetworkDock.html) | Send robot to charger (charger must be in the driveable space of the map) | -| execute_task_action | [clearpath_task_msgs/action/ExecuteTask](pathname:///api/html/clearpath_task_msgs/action/ExecuteTask.html) | Run a task as an action | diff --git a/docs_outdoornav_user_manual/api/api_endpoints/docking/_category_.json b/docs_outdoornav_user_manual/api/api_endpoints/docking/_category_.json new file mode 100644 index 00000000..aad718b9 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/docking/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Docking", + "position": 3 +} diff --git a/docs_outdoornav_user_manual/api/api_endpoints/docking/add_dock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/docking/add_dock.mdx new file mode 100644 index 00000000..8f680628 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/docking/add_dock.mdx @@ -0,0 +1,111 @@ +--- +title: Create New Dock +sidebar_label: Create Dock +sidebar_position: 1 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Create a new dock. Dock information is stored in persistent memory. + +- Service Name: **docking/dock_manager/add_dock** + +- Service Type: [clearpath_dock_msgs/srv/AddDock](pathname:///api/html/clearpath_dock_msgs/srv/AddDock.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_dock_msgs.srv import AddDock + + +class AddDock(Node): + + def __init__(self): + super().__init__('add_dock_client') + self._srv_client = self.create_client(AddDock, 'docking/dock_manager/add_dock') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = AddDock.Request() + + def send_request(self, lat, lon): + # self.req.lat = lat + # self.req.lon = lon + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + add_dock_client = AddDock() + response = add_dock_client.send_request() + if response.success: + add_dock_client.get_logger().info('New dock created succesfully!') + else: + add_dock_client.get_logger().error('Failed creating new dock') + + set_datum_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_dock_msgs/srv/add_dock.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_dock_client"); + rclcpp::Client::SharedPtr client = + node->create_client("docking/dock_manager/add_dock"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "New dock created!"); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to create new dock!"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service docking/dock_manager/add_dock"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/docking/get_dock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/docking/get_dock.mdx new file mode 100644 index 00000000..0bcd4b61 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/docking/get_dock.mdx @@ -0,0 +1,110 @@ +--- +title: Get Dock Info +sidebar_label: Get Dock Information +sidebar_position: 2 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Retrieve the dock information + +- Service Name: **docking/dock_manager/get_dock** + +- Service Type: [clearpath_dock_msgs/srv/GetDock](pathname:///api/html/clearpath_dock_msgs/srv/GetDock.html) + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_dock_msgs.srv import GetDock + + +class GetDock(Node): + + def __init__(self): + super().__init__('get_dock_client') + self._srv_client = self.create_client(GetDock, 'docking/dock_manager/get_dock') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = GetDock.Request() + + def send_request(self, lat, lon): + # self.req.lat = lat + # self.req.lon = lon + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + get_dock_client = Getock() + response = get_dock_client.send_request() + if response.success: + get_dock_client.get_logger().info('Retrived dock info succesfully!') + else: + get_dock_client.get_logger().error('Failed to retrieve dock info') + + get_dock_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_dock_msgs/srv/get_dock.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("get_dock_client"); + rclcpp::Client::SharedPtr client = + node->create_client("docking/dock_manager/get_dock"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Retrived dock info successfully"); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to retrieve dock info"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service docking/dock_manager/get_dock"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/docking/survery_dock.mdx b/docs_outdoornav_user_manual/api/api_endpoints/docking/survery_dock.mdx new file mode 100644 index 00000000..7eea1643 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/docking/survery_dock.mdx @@ -0,0 +1,112 @@ +--- +title: Survey Dock +sidebar_label: Survey Dock +sidebar_position: 3 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Survey the location of an existing dock. This is required to accurately position the dock on the map and +ensure that proper docking is achieved. The dock target must be clearly visible by the docking lidar for +this survey process to succeed. + +- Service Name: **docking/dock_localizer/survey_dock** + +- Service Type: [clearpath_dock_msgs/srv/SurveyDock](pathname:///api/html/clearpath_dock_msgs/srv/SurveyDock.html) + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_dock_msgs.srv import SurveyDock + + +class SurveyDock(Node): + + def __init__(self): + super().__init__('survey_dock_client') + self._srv_client = self.create_client(SurveyDock, 'docking/dock_localizer/survey_dock') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SurveyDock.Request() + + def send_request(self, lat, lon): + # self.req.lat = lat + # self.req.lon = lon + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + survey_dock_client = SurveyDock() + response = survey_dock_client.send_request() + if response.success: + survey_dock_client.get_logger().info('Dock was surveyed successfully!') + else: + survey_dock_client.get_logger().error('Failed to survey dock') + + survey_dock_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_dock_msgs/srv/survey_dock.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("survey_dock_client"); + rclcpp::Client::SharedPtr client = + node->create_client("docking/dock_localizer/survey_dock"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Surveyed dock successfully"); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to survey dock"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service docking/dock_localizer/survey_dock"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/localization/_category_.json b/docs_outdoornav_user_manual/api/api_endpoints/localization/_category_.json new file mode 100644 index 00000000..f7b3ccaf --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/localization/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Localization", + "position": 2 +} diff --git a/docs_outdoornav_user_manual/api/api_endpoints/localization/monitor_location.mdx b/docs_outdoornav_user_manual/api/api_endpoints/localization/monitor_location.mdx new file mode 100644 index 00000000..aa5ef936 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/localization/monitor_location.mdx @@ -0,0 +1,165 @@ +--- +title: Monitor Location +sidebar_label: Monitor Location +sidebar_position: 1 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +## Map location + +The XY location of the platform in the 'map' coordinate frame. These values are relative to the datum coorodinate. + +- Topic Name: **localization/odom** + +- Topic Type: [nav_msgs/msg/Odometry](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Odometry.msg) + +- Topic QoS: + + + + +```python +import rclpy +from rclpy.node import Node + +from nav_msgs.msg import Odometry + + +class LocalizationOdom(Node): + + def __init__(self): + super().__init__('localization_odom_sub') + self.odom_sub = self.create_subscription( + Odometry, 'localization/odom', self.odom_cb, 10) + self.odom_sub # prevent unused variable warning + + def odom_cb(self, msg): + self.last_odom_msg = msg + +if __name__ == '__main__': + rclpy.init(args=args) + localization_odom_sub = LocalizationOdom() + rclpy.spin(localization_odom_sub) + localization_odom_sub.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/odometry.hpp" + +class LocalizationOdom : public rclcpp::Node +{ +public: + LocalizationOdom() + : Node("localization_odom_sub") + { + auto odom_cb = + [this](nav_msgs::msg::Odometry::UniquePtr msg) -> void { + last_odom_msg_ = msg; + }; + + odom_sub_ = this->create_subscription("localization/odom", 10, odom_cb); + } + +private: + rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::Odometry::UniquePtr last_odom_msg_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +``` + + + + +## Lat/Lon location + +The lat/lon location of the platform. + +- Topic Name: **localization/fix** + +- Topic Type: [sensor_msgs/msg/NavSatFix](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Odometry.msg) + +- Topic QoS: + + + + +```python +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import NavSatFix + + +class LocalizationFix(Node): + + def __init__(self): + super().__init__('localization_fix_sub') + self.odom_sub = self.create_subscription( + NavSatFix, 'localization/fix', self.fix_cb, 10) + self.fix_sub # prevent unused variable warning + + def fix_cb(self, msg): + self.last_fix_msg = msg + +if __name__ == '__main__': + rclpy.init(args=args) + localization_fix_sub = LocalizationFix() + rclpy.spin(localization_fix_sub) + localization_fix_sub.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +class LocalizationFix : public rclcpp::Node +{ +public: + LocalizationFix() + : Node("localization_fix_sub") + { + auto odom_cb = + [this](sensor_msgs::msg::NavSatFix::UniquePtr msg) -> void { + last_fix_msg_ = msg; + }; + + fix_sub_ = this->create_subscription("localization/fix", 10, fix_cb); + } + +private: + rclcpp::Subscription::SharedPtr fix_sub_; + sensor_msgs::msg::NavSatFix::UniquePtr last_fix_msg_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/localization/set_datum.mdx b/docs_outdoornav_user_manual/api/api_endpoints/localization/set_datum.mdx new file mode 100644 index 00000000..50b92ea2 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/localization/set_datum.mdx @@ -0,0 +1,118 @@ +--- +title: Set Datum +sidebar_label: Set Datum +sidebar_position: 2 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Set the datum. The datum is the reference coordinate of the navigations "map" frame. +The XY position of the robot is relative to this location. + +- Service Name: **localization/set_datum** + +- Service Type: [clearpath_localization_msgs/srv/SetDatum](pathname:///api/html/clearpath_localization_msgs/srv/SetDatum.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from clearpath_localization_msgs.srv import SetDatum + +DATUM_LAT = 43.500049591064453 +DATUM_LON = -80.546844482421875 + +class SetDatum(Node): + + def __init__(self): + super().__init__('set_datum_client') + self._srv_client = self.create_client(SetDatum, 'localization/set_datum') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SetDatum.Request() + + def send_request(self, lat, lon): + self.req.lat = lat + self.req.lon = lon + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + set_datum_client = SetDatum() + response = set_datum_client.send_request(DATUM_LAT, DATUM_LON) + if response.success: + set_datum_client.get_logger().info('Datum set succesfully: (%s, %s)' % (DATUM_LAT, DATUM_LON)) + else: + set_datum_client.get_logger().error('Set datum failed!') + + set_datum_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "clearpath_localization_msgs/srv/set_datum.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +const double DATUM_LAT = 43.500049591064453; +const double DATUM_LON = -80.546844482421875; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("set_datum_client"); + rclcpp::Client::SharedPtr client = + node->create_client("localization/set_datum"); + + auto request = std::make_shared(); + request->lat = DATUM_LAT; + request->lon = DATUM_LON; + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Datum set successfully: (%s, %s)", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Set datum failed!"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service localization/set_datum"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/logger/_category_.json b/docs_outdoornav_user_manual/api/api_endpoints/logger/_category_.json new file mode 100644 index 00000000..f848435e --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/logger/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Mission Logger", + "position": 4 +} diff --git a/docs_outdoornav_user_manual/api/api_endpoints/logger/start_recording.mdx b/docs_outdoornav_user_manual/api/api_endpoints/logger/start_recording.mdx new file mode 100644 index 00000000..d0efdb06 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/logger/start_recording.mdx @@ -0,0 +1,108 @@ +--- +title: Start Recording +sidebar_label: Start Recording +sidebar_position: 1 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Start recording mission data. + +- Service Name: **log_manager/start_recording** + +- Service Type: [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import Trigger + + +class StartLogger(Node): + + def __init__(self): + super().__init__('start_logger_client') + self._srv_client = self.create_client(Trigger, 'log_manager/start_recording') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = Trigger.Request() + + def send_request(self): + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + start_logger_client = StartLogger() + response = start_logger_client.send_request() + if response.success: + start_logger_client.get_logger().info('Logger started recording')) + else: + start_logger_client.get_logger().error('Failed to start logger recording') + + start_logger_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("start_logger_client"); + rclcpp::Client::SharedPtr client = + node->create_client("log_manager/start_recording"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Logger started recording", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to start logger recording"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service log_manager/start_recording"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/logger/stop_recording.mdx b/docs_outdoornav_user_manual/api/api_endpoints/logger/stop_recording.mdx new file mode 100644 index 00000000..eee8fa03 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/logger/stop_recording.mdx @@ -0,0 +1,108 @@ +--- +title: Stop Recording +sidebar_label: Stop Recording +sidebar_position: 2 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Stop recording mission data. + +- Service Name: **log_manager/stop_recording** + +- Service Type: [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import Trigger + + +class StopLogger(Node): + + def __init__(self): + super().__init__('stop_logger_client') + self._srv_client = self.create_client(Trigger, 'log_manager/stop_recording') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = Trigger.Request() + + def send_request(self): + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + stop_logger_client = StopLogger() + response = stop_logger_client.send_request() + if response.success: + stop_logger_client.get_logger().info('Logger started recording')) + else: + stop_logger_client.get_logger().error('Failed to start logger recording') + + stop_logger_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("stop_logger_client"); + rclcpp::Client::SharedPtr client = + node->create_client("log_manager/stop_recording"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Logger stopped recording", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to stop logger recording"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service log_manager/stop_recording"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/_category_.json b/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/_category_.json new file mode 100644 index 00000000..ac6a2683 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Mission Manager", + "position": 5 +} diff --git a/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/stop_recording.mdx b/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/stop_recording.mdx new file mode 100644 index 00000000..eee8fa03 --- /dev/null +++ b/docs_outdoornav_user_manual/api/api_endpoints/mission_manager/stop_recording.mdx @@ -0,0 +1,108 @@ +--- +title: Stop Recording +sidebar_label: Stop Recording +sidebar_position: 2 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +Stop recording mission data. + +- Service Name: **log_manager/stop_recording** + +- Service Type: [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) + + + + + +```python +import rclpy +from rclpy.node import Node + +from std_srvs.srv import Trigger + + +class StopLogger(Node): + + def __init__(self): + super().__init__('stop_logger_client') + self._srv_client = self.create_client(Trigger, 'log_manager/stop_recording') + while not self._srv_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = Trigger.Request() + + def send_request(self): + self.future = self._srv_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +if __name__ == '__main__': + rclpy.init(args=args) + stop_logger_client = StopLogger() + response = stop_logger_client.send_request() + if response.success: + stop_logger_client.get_logger().info('Logger started recording')) + else: + stop_logger_client.get_logger().error('Failed to start logger recording') + + stop_logger_client.destroy_node() + rclpy.shutdown() +``` + + + +```cpp +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("stop_logger_client"); + rclcpp::Client::SharedPtr client = + node->create_client("log_manager/stop_recording"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->success) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Logger stopped recording", ); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Failed to stop logger recording"); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service log_manager/stop_recording"); + } + + rclcpp::shutdown(); + return 0; +} +``` + + + diff --git a/docs_outdoornav_user_manual/api/api_overview.mdx b/docs_outdoornav_user_manual/api/api_overview.mdx index 92e2c273..dc2b3dfe 100644 --- a/docs_outdoornav_user_manual/api/api_overview.mdx +++ b/docs_outdoornav_user_manual/api/api_overview.mdx @@ -15,26 +15,11 @@ flexibility to do so. The API is, at present, a [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/index.html) API. The API is divided into two sections, whose details are provided below: -- [Autonomy API](./api_endpoints/autonomy_api): The set of [ROS +- Autonomy API: The set of [ROS Topics](http://wiki.ros.org/Topics) that are used for monitoring and controlling the the hardware platform through the OutdoorNav autonomy software. - - [Topics Subscribed to by Autonomy](./api_endpoints/autonomy_api.mdx#autonomy-subscribers): - The set of [ROS Topics](http://wiki.ros.org/Topics) - subscribed to by OutdoorNav Software, typically published by the - client for directing OutdoorNav operation. - - [Topics Published by Autonomy](./api_endpoints/autonomy_api.mdx#autonomy-publishers): - The set of [ROS Topics](http://wiki.ros.org/Topics) published by - OutdoorNav Software, to be subscribed to by the UGV. - - [Services Exported by Autonomy](./api_endpoints/autonomy_api#autonomy-services): - The set of [ROS Services](http://wiki.ros.org/Services) provided - by OutdoorNav Software, for use by the client to modify/control - the behaviour of the Autonomy. - - [Actions Exported by Autonomy](./api_endpoints/autonomy_api#autonomy-actions): - The set of [ROS Actions](http://wiki.ros.org/actionlib) provided - by OutdoorNav Software, for use by the client to modify/control - the behaviour of the Autonomy. -- [Mission Manager API](./api_endpoints/mission_manager_api): The set of [ROS +- Mission Manager API: The set of [ROS Services](http://wiki.ros.org/Services) that are used for creating, deleting, and modifying OutdoorNav Missions - [API Examples](./api_examples/api_examples_overview): Example code to come. diff --git a/docs_outdoornav_user_manual/api/autonomy_api.mdx b/docs_outdoornav_user_manual/api/autonomy_api.mdx new file mode 100644 index 00000000..e5887bba --- /dev/null +++ b/docs_outdoornav_user_manual/api/autonomy_api.mdx @@ -0,0 +1,63 @@ +--- +title: Autonomy API Endpoints +sidebar_label: Autonomy API +sidebar_position: 4 +toc_min_heading_level: 2 +toc_max_heading_level: 4 +--- +import Tabs from "@theme/Tabs"; +import TabItem from "@theme/TabItem"; + +:::note + +All nodes, topics, and services are namespace prefixed with the robot serial number. + +If your serial number is `cpr-a300-00001`, then your namespace +will be `a300_00001`. This can be overwritten in the [robot.yaml](../../../../docs/ros/config/yaml/overview) file. + +::: + + +  + +## Publishers {#autonomy-publishers} + +  + +| Topic | Message type | Description | QoS | +| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | :-------------------------------------------- | +| autonomy/config | [clearpath_navigation_msgs/msg/AutonomyConfig](pathname:///api/html/clearpath_navigation_msgs/msg/AutonomyConfig.html) | Autonomy configuration | [System Default](/docs/ros/api/overview#system-default) | +| autonomy/initial_path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Initial Path computed by autonomy | [System Default](/docs/ros/api/overview#system-default) | +| autonomy/status | [clearpath_navigation_msgs/msg/AutonomyStatus](pathname:///api/html/clearpath_navigation_msgs/msg/AutonomyStatus.html) | Status of the autonomy | [System Default](/docs/ros/api/overview#system-default) | +| control_selection/current_mode | [clearpath_control_msgs/msg/ControlMode](pathname:///api/html/clearpath_control_msgs/msg/ControlMode.html) | Current control mode (NEUTRAL, MANUAL, AUTONOMY). | [System Default](/docs/ros/api/overview#system-default) | +| control_selection/control_state | [clearpath_control_msgs/msg/ControlSelectionState](pathname:///api/html/clearpath_control_msgs/msg/ControlSelectionState.html) | Complete state of control selection node. | [System Default](/docs/ros/api/overview#system-default) | +| docking/docking_server/path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Docking path | +| docking/undocking_server/path | [nav_msgs/msg/Path](https://github.com/ros2/common_interfaces/blob/jazzy/nav_msgs/msg/Path.msg) | Undocking path | +| goto/preview | [clearpath_navigation_msgs/msg/GoToPreview](pathname:///api/html/clearpath_navigation_msgs/msg/GoToPreview.html) | Preview points for a GoTo execution | [System Default](/docs/ros/api/overview#system-default) | +| mission/preview | [clearpath_navigation_msgs/msg/MissionPreview](pathname:///api/html/clearpath_navigation_msgs/msg/MissionPreview.html) | Preview points for a mission execution | [System Default](/docs/ros/api/overview#system-default) | +| speed_limit | [clearpath_navigation_msgs/msg/GoToPreview](https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg) | Current navigation speed limit | [System Default](/docs/ros/api/overview#system-default) | +| ui/heartbeat | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Heartbeat of the UI | [System Default](/docs/ros/api/overview#system-default) | +| ui_teleop/cmd_vel | [geometry_msgs/msg/TwistStamped](https://github.com/ros2/common_interfaces/blob/jazzy/geometry_msgs/msg/TwistStamped.msg) | Velocity commands fron the UI joystick | [System Default](/docs/ros/api/overview#system-default) | + +  + +## Services {#autonomy-services} + +  + +| Service | Service type | Description | +| :---------------------- | :----------------------------------------------------------------------------------------------------------- | :-------------- | +| docking/dock_manager/clear_data | [std_srvs/srv/Trigger](https://docs.ros2.org/foxy/api/std_srvs/srv/Trigger.html) | Clear all dock data | +| docking/dock_manager/delete_dock | [clearpath_dock_msgs/srv/RemoveDock](pathname:///api/html/clearpath_dock_msgs/srv/RemoveDock.html) | Delete a dock | +| docking/dock_manager/export | [clearpath_dock_msgs/srv/ExportData](pathname:///api/html/clearpath_dock_msgs/srv/ExportData.html) | Export dock data | +| docking/dock_manager/get_database | [clearpath_dock_msgs/srv/GetDockDatabase](pathname:///api/html/clearpath_dock_msgs/srv/GetDockDatabase.html) | Returns the entire dock database | +| docking/dock_manager/import | [clearpath_dock_msgs/srv/ImportData](pathname:///api/html/clearpath_dock_msgs/srv/ImportData.html) | Import a dock | +| docking/dock_manager/update_dock | [clearpath_dock_msgs/srv/UpdateDock](pathname:///api/html/clearpath_dock_msgs/srv/UpdateDock.html) | Update a docks information | +| docking/dock_localizer/add_dock_current_pose| [clearpath_dock_msgs/srv/AddDockCurrentPose](pathname:///api/html/clearpath_dock_msgs/srv/AddDockCurrentPose.html) | Add a dock with the current pose | +| docking/dock_localizer/get_dock_poses| [clearpath_dock_msgs/srv/GetDockPoses](pathname:///api/html/clearpath_dock_msgs/srv/GetDockPoses.html) | Return the dock and predock poses of a specific dock | + +| localization/lat_lon_to_xy | [clearpath_localization_msgs/srv/ConvertLatLonToCartesian](pathname:///api/html/clearpath_localization_msgs/srv/ConvertLatLonToCartesian.html) | Convert lat/lon condinate to map XY coordinate | +| localization/lat_lon_to_xy_array | [clearpath_localization_msgs/srv/ConvertLatLonToCartesianArray](pathname:///api/html/clearpath_localization_msgs/srv/ConvertLatLonToCartesianArray.html) | Convert a set of lat/lon coordinates to map XY coordinates | +| localization/xy_to_lat_lon | [clearpath_localization_msgs/srv/ConvertCartesianToLatLon](pathname:///api/html/clearpath_localization_msgs/srv/ConvertCartesianToLatLon.html) | Convert map XY coordinate to lat/lon coordinate | +| localization/xy_to_lat_lon_array | [clearpath_localization_msgs/srv/ConvertCartesianToLatLonArray](pathname:///api/html/clearpath_localization_msgs/srv/ConvertCartesianToLatLonArray.html) | Convert a set of map XY coordinates to lat/lon coordinates | +| log_manager/delete_log | [clearpath_logger_msgs/srv/DeleteLog](pathname:///api/html/clearpath_logger_msgs/srv/DeleteLog.html) | Delete a secific log | diff --git a/docs_outdoornav_user_manual/api/api_endpoints/mission_manager_api.mdx b/docs_outdoornav_user_manual/api/mission_manager_api.mdx similarity index 99% rename from docs_outdoornav_user_manual/api/api_endpoints/mission_manager_api.mdx rename to docs_outdoornav_user_manual/api/mission_manager_api.mdx index 3aacfcc1..25864fcd 100644 --- a/docs_outdoornav_user_manual/api/api_endpoints/mission_manager_api.mdx +++ b/docs_outdoornav_user_manual/api/mission_manager_api.mdx @@ -1,7 +1,7 @@ --- title: Mission Manager API Endpoints sidebar_label: Mission Manager API -sidebar_position: 3 +sidebar_position: 5 toc_min_heading_level: 2 toc_max_heading_level: 4 --- diff --git a/docusaurus.config.js b/docusaurus.config.js index 686e00c3..cc7abc75 100644 --- a/docusaurus.config.js +++ b/docusaurus.config.js @@ -105,7 +105,7 @@ const config = { remarkPlugins: [remarkMath], rehypePlugins: [rehypeKatex], showLastUpdateTime: true, - includeCurrentVersion: false, + includeCurrentVersion: true, }, ], [