|
@@ -234,9 +234,9 @@ self.joint_states.position = [0.0,0.0]
|
|
|
|
|
|
```python
|
|
```python
|
|
delta_time = time.time()-last_update_time
|
|
delta_time = time.time()-last_update_time
|
|
-current_position = self.joint_states.position
|
|
|
|
-self.joint_states.position[0] += delta_time*current_position[0]
|
|
|
|
-self.joint_states.position[1] += delta_time*current_position[1]
|
|
|
|
|
|
+# 更新位置
|
|
|
|
+self.joint_states.position[0] += delta_time*self.joint_states.velocity[0]
|
|
|
|
+self.joint_states.position[1] += delta_time*self.joint_states.velocity[1]
|
|
```
|
|
```
|
|
|
|
|
|
#### 3.2.3 velocity
|
|
#### 3.2.3 velocity
|
|
@@ -295,15 +295,16 @@ class RotateWheelNode(Node):
|
|
last_update_time = time.time()
|
|
last_update_time = time.time()
|
|
while rclpy.ok():
|
|
while rclpy.ok():
|
|
delta_time = time.time()-last_update_time
|
|
delta_time = time.time()-last_update_time
|
|
|
|
+ last_update_time = time.time()
|
|
# 更新位置
|
|
# 更新位置
|
|
- current_position = self.joint_states.position
|
|
|
|
- self.joint_states.position[0] += delta_time*current_position[0]
|
|
|
|
- self.joint_states.position[1] += delta_time*current_position[1]
|
|
|
|
|
|
+ self.joint_states.position[0] += delta_time*self.joint_states.velocity[0]
|
|
|
|
+ self.joint_states.position[1] += delta_time*self.joint_states.velocity[1]
|
|
# 更新速度
|
|
# 更新速度
|
|
self.joint_states.velocity = self.joint_speeds
|
|
self.joint_states.velocity = self.joint_speeds
|
|
|
|
+ # 更新 header
|
|
|
|
+ self.joint_states.header.stamp = self.get_clock().now().to_msg()
|
|
# 发布关节数据
|
|
# 发布关节数据
|
|
self.joint_states_publisher_.publish(self.joint_states)
|
|
self.joint_states_publisher_.publish(self.joint_states)
|
|
- last_update_time = time.time()
|
|
|
|
self.pub_rate.sleep()
|
|
self.pub_rate.sleep()
|
|
|
|
|
|
def main(args=None):
|
|
def main(args=None):
|
|
@@ -332,7 +333,7 @@ ros2 run fishbot_description rotate_wheel
|
|
测试之前还需要修改下`display_rviz2.launch.py`文件,注释其`joint_state_publisher`节点
|
|
测试之前还需要修改下`display_rviz2.launch.py`文件,注释其`joint_state_publisher`节点
|
|
|
|
|
|
```
|
|
```
|
|
-# ld.add_action(joint_state_publisher_node)x
|
|
|
|
|
|
+# ld.add_action(joint_state_publisher_node)
|
|
ld.add_action(robot_state_publisher_node)
|
|
ld.add_action(robot_state_publisher_node)
|
|
ld.add_action(rviz2_node)
|
|
ld.add_action(rviz2_node)
|
|
```
|
|
```
|