change

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -91,40 +91,40 @@ def tearDownClass(cls):
 
     def command_velocity_callback(self, msg):
         print('command_velocity_callback Start')
-        # self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
-        # self.command = msg
+        self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
+        self.command = msg
         print('command_velocity_callback OK')
 
     def timer_callback(self):
         # Propagate command
         print('timer_callback Start')
-        # period = 0.05
-        # self.x += cos(self.theta) * self.command.linear.x * period
-        # self.y += sin(self.theta) * self.command.linear.x * period
-        # self.theta += self.command.angular.z * period
-        # # Need to publish updated TF
-        # self.publish()
+        period = 0.05
+        self.x += cos(self.theta) * self.command.linear.x * period
+        self.y += sin(self.theta) * self.command.linear.x * period
+        self.theta += self.command.angular.z * period
+        # Need to publish updated TF
+        self.publish()
         print('timer_callback OK')
 
     def publish(self):
         # Publish base->odom transform
         print('publish Start')
-        # t = TransformStamped()
-        # t.header.stamp = self.node.get_clock().now().to_msg()
-        # t.header.frame_id = 'odom'
-        # t.child_frame_id = 'base_link'
-        # t.transform.translation.x = self.x
-        # t.transform.translation.y = self.y
-        # t.transform.rotation.z = sin(self.theta / 2.0)
-        # t.transform.rotation.w = cos(self.theta / 2.0)
-        # self.tf_broadcaster.sendTransform(t)
-        # # Publish battery state
-        # b = BatteryState()
-        # if self.is_charging:
-        #     b.current = 1.0
-        # else:
-        #     b.current = -1.0
-        # self.battery_state_pub.publish(b)
+        t = TransformStamped()
+        t.header.stamp = self.node.get_clock().now().to_msg()
+        t.header.frame_id = 'odom'
+        t.child_frame_id = 'base_link'
+        t.transform.translation.x = self.x
+        t.transform.translation.y = self.y
+        t.transform.rotation.z = sin(self.theta / 2.0)
+        t.transform.rotation.w = cos(self.theta / 2.0)
+        self.tf_broadcaster.sendTransform(t)
+        # Publish battery state
+        b = BatteryState()
+        if self.is_charging:
+            b.current = 1.0
+        else:
+            b.current = -1.0
+        self.battery_state_pub.publish(b)
         print('publish OK')
 
     def action_goal_callback(self, future):
@@ -153,13 +153,13 @@ def action_feedback_callback(self, msg):
 
     def nav_execute_callback(self, goal_handle):
         print('nav_execute_callback Start')
-        # goal = goal_handle.request
-        # self.x = goal.pose.pose.position.x - 0.05
-        # self.y = goal.pose.pose.position.y + 0.05
-        # self.theta = 2.0 * acos(goal.pose.pose.orientation.w)
-        # self.node.get_logger().info('Navigating to %f %f %f' % (self.x, self.y, self.theta))
-        # goal_handle.succeed()
-        # self.publish()
+        goal = goal_handle.request
+        self.x = goal.pose.pose.position.x - 0.05
+        self.y = goal.pose.pose.position.y + 0.05
+        self.theta = 2.0 * acos(goal.pose.pose.orientation.w)
+        self.node.get_logger().info('Navigating to %f %f %f' % (self.x, self.y, self.theta))
+        goal_handle.succeed()
+        self.publish()
 
         result = NavigateToPose.Result()
         result.error_code = 0
@@ -231,12 +231,12 @@ def test_docking_server(self):
 
         print('test_docking_server dock preempt OK')
 
-        # Wait until we get the dock preemption
-        while len(self.action_result) == 0:
-            rclpy.spin_once(self.node, timeout_sec=0.1)
-            time.sleep(0.1)
+        # # Wait until we get the dock preemption
+        # while len(self.action_result) == 0:
+        #     rclpy.spin_once(self.node, timeout_sec=0.1)
+        #     time.sleep(0.1)
 
-        print('test_docking_server dock prempt spin OK')
+        # print('test_docking_server dock prempt spin OK')
 
         # assert self.action_result[0].status == GoalStatus.STATUS_ABORTED
         # assert not self.action_result[0].result.success