... | @@ -108,19 +108,87 @@ node = rclpy.create_node('lab2') |
... | @@ -108,19 +108,87 @@ node = rclpy.create_node('lab2') |
|
publisher = node.create_publisher(geometry_msgs.msg.Twist, topic, 10)
|
|
publisher = node.create_publisher(geometry_msgs.msg.Twist, topic, 10)
|
|
|
|
|
|
# Create a message to publish
|
|
# Create a message to publish
|
|
twist = geometry_msgs.msg.Twist()
|
|
twist_msg = geometry_msgs.msg.Twist()
|
|
twist.... # Check the
|
|
twist_msg.linear... # Check the documentation for twist and complete, you can publish to move linearly at 0.1m/s and turn by 0.05rad/s
|
|
|
|
|
|
|
|
# publish the message
|
|
|
|
publisher.publish(twist_msg)
|
|
|
|
|
|
|
|
rclpy.spin()
|
|
```
|
|
```
|
|
|
|
|
|
The documentation for ```geometry_msgs/Twist``` can be found online at [https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html](geometry_msgs/Twist) or by running ```ros2 msg info geometry_msgs/Twist```.
|
|
The documentation for a ```node``` can be found at [rclpy.node](https://docs.ros2.org/galactic/api/rclpy/api/node.html). The documentation for ```geometry_msgs/Twist``` can be found online at [geometry_msgs/Twist](https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html) or by running ```ros2 msg info geometry_msgs/Twist```.
|
|
|
|
|
|
|
|
When you run your node, you will notice that your robot move a little bit and then stop, we need to constantly publish the velocity.
|
|
|
|
|
|
|
|
```python
|
|
|
|
import time
|
|
|
|
while rclpy.ok():
|
|
|
|
publisher.publish(twist_msg)
|
|
|
|
rclpy.spin_once()
|
|
|
|
time.sleep(0.1)
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
### C+++
|
|
### C+++
|
|
|
|
|
|
|
|
Listen on a topic
|
|
|
|
-----------------
|
|
|
|
|
|
|
|
In the previous part, we have created a node that publish on a topic and make your robot move, but it never stops. In this part, we will listen to the ```/odom``` topic and use that information to stop when the robot has moved more than 1.0 meter away from its start position.
|
|
|
|
|
|
|
|
### Python
|
|
|
|
|
|
|
|
To subscribe to a topic, we need to define a *callback* function that will be called every time a message is received, and we need to create a subscriber:
|
|
|
|
|
|
|
|
```python
|
|
|
|
def callback_odometry(msg):
|
|
|
|
print("Received: ", msg)
|
|
|
|
|
|
|
|
subscription = node.create_subscription(nav_msgs.msg.Odometry, callback_odometry, 1)
|
|
|
|
|
|
|
|
```
|
|
|
|
|
|
|
|
You might have to import a package for the message definition. In the ```callback_odometry``` function, you should measure the distance from the start location and when it is larger than 1.0 meter, stop the execution.
|
|
|
|
|
|
|
|
### C++
|
|
|
|
|
|
|
|
Parameters
|
|
|
|
----------
|
|
|
|
|
|
|
|
For now we have hard coded parameters, such as the velocity and the distance. To make our node more effective, we should set those as parameters on the command line.
|
|
|
|
|
|
|
|
So that we can run our program this way:
|
|
|
|
|
|
|
|
|
|
|
|
```bash
|
|
|
|
ros2 run lab2 lab2_node
|
|
|
|
```
|
|
|
|
|
|
|
|
### Python
|
|
|
|
|
|
|
|
Create a parameter called ```linear``` with default value ```0.1``` with the following:
|
|
|
|
|
|
|
|
```python
|
|
|
|
param_linear_velocity = node.declare_parameter("linear", 0.1)
|
|
|
|
```
|
|
|
|
|
|
|
|
Access the value using the ```get_parameter_value()``` function:
|
|
|
|
|
|
|
|
```python
|
|
|
|
print(param_linear_velocity.get_parameter_value())
|
|
|
|
```
|
|
|
|
|
|
|
|
Create the other two parameters ```angular``` and ```distance```. And use the parameter values instead of the hard coded values.
|
|
|
|
|
|
|
|
### C++
|
|
|
|
|
|
|
|
|
|
|
|
Exploration with a random pattern
|
|
|
|
---------------------------------
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Starting an action
|
|
|
|
------------------
|
|
|
|
|
|
|
|
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 6.0, y: 2.0, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"
|
|
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 6.0, y: 2.0, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"
|
|
|
|
|
... | | ... | |