... | @@ -288,14 +288,92 @@ entry_points={ |
... | @@ -288,14 +288,92 @@ entry_points={ |
|
},
|
|
},
|
|
```
|
|
```
|
|
|
|
|
|
|
|
We will use the ```NavigateToPose``` action, you can check its type with:
|
|
|
|
|
|
|
|
```bash
|
|
ros2 interface show nav2_msgs/action/NavigateToPose
|
|
ros2 interface show nav2_msgs/action/NavigateToPose
|
|
|
|
```
|
|
|
|
|
|
|
|
Before starting the implementation, study the [official action tutorial](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html)
|
|
|
|
|
|
|
|
You can use the following in ```random_exploration.py``` to go to a single location:
|
|
|
|
|
|
|
|
```python
|
|
|
|
import rclpy
|
|
|
|
import math
|
|
|
|
from rclpy.action import ActionClient
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
|
|
|
from nav2_msgs.action import NavigateToPose
|
|
|
|
|
|
|
|
|
|
|
|
class RandomExploration(Node):
|
|
|
|
|
|
|
|
def __init__(self):
|
|
|
|
super().__init__('random_exploration')
|
|
|
|
self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
|
|
|
|
|
|
|
|
def send_goal(self, x, y, angle):
|
|
|
|
goal_msg = NavigateToPose.Goal()
|
|
|
|
goal_msg.pose.header.frame_id = "map"
|
|
|
|
goal_msg.pose.pose.position.x = x
|
|
|
|
goal_msg.pose.pose.position.y = y
|
|
|
|
goal_msg.pose.pose.orientation.w = math.cos(angle/2)
|
|
|
|
goal_msg.pose.pose.orientation.z = math.sin(angle/2)
|
|
|
|
|
|
|
|
self._action_client.wait_for_server()
|
|
|
|
|
|
|
|
self._send_goal_future = self._action_client.send_goal_async(goal_msg, 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):
|
|
|
|
rclpy.shutdown()
|
|
|
|
|
|
|
|
def feedback_callback(self, feedback_msg):
|
|
|
|
print(feedback_msg.feedback)
|
|
|
|
self.get_logger().info('Received feedback: {0}'.format(feedback_msg.feedback.navigation_time))
|
|
|
|
if feedback_msg.feedback.navigation_time.sec > 60:
|
|
|
|
self._action_client.cancel_goal()
|
|
|
|
rclpy.shutdown()
|
|
|
|
|
|
|
|
|
|
|
|
def main(args=None):
|
|
|
|
rclpy.init(args=args)
|
|
|
|
|
|
|
|
action_client = RandomExploration()
|
|
|
|
|
|
|
|
action_client.send_goal(10.0, 5.0, 1.0)
|
|
|
|
|
|
|
|
rclpy.spin(action_client)
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
main()
|
|
|
|
```
|
|
|
|
|
|
|
|
Extend that class so that you keep going to a new random location, either when the robot has reached its current location, or when it is failing to reach it.
|
|
|
|
|
|
### C++
|
|
### C++
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Demonstration
|
|
|
|
--------------
|
|
|
|
|
|
|
|
* run your lab2_node and random_exploration nodes
|
|
|
|
* show your screen file and that the map is loaded correctly
|
|
|
|
|
|
|
|
|
|
|
|
|