11#!/usr/bin/env python3
22
3+ import os
4+
35import rclpy
4- from rclpy .node import Node
56import yaml
7+ from ament_index_python .packages import get_package_share_directory
8+ from control_msgs .action import FollowJointTrajectory
9+ from geometry_msgs .msg import Pose , PoseArray
10+ from rclpy .action import ActionClient
11+ from rclpy .node import Node
12+ from trajectory_msgs .msg import JointTrajectory
613
7- from snp_msgs .srv import GenerateMotionPlan
814from snp_msgs .msg import ToolPath
9- from geometry_msgs .msg import PoseArray , Pose
10- from trajectory_msgs .msg import JointTrajectory
11- from rclpy .action import ActionClient
12- from control_msgs .action import FollowJointTrajectory
15+ from snp_msgs .srv import GenerateMotionPlan
1316
1417
1518def create_dummy_tool_path () -> ToolPath :
@@ -132,7 +135,7 @@ def request_control_result(self, future):
132135
133136 def get_result_callback (self , future ):
134137 result = future .result ().result
135- self .get_logger ().info (f"Result: { result .error_code } " )
138+ self .get_logger ().info (f"Result: { result .error_string } " )
136139
137140 def feedback_callback (self , feedback_msg ):
138141 feedback = feedback_msg .feedback
@@ -164,7 +167,9 @@ def main(args=None):
164167 client .get_logger ().info ("[plan] Received motion plan successfully!" )
165168 client .get_logger ().info (f"[plan] Approach: { len (response .approach .points )} points" )
166169 client .get_logger ().info (f"[plan] Process: { len (response .process .points )} points" )
167- client .get_logger ().info (f"[plan] Departure: { len (response .departure .points )} points" )
170+ client .get_logger ().info (
171+ f"[plan] Departure: { len (response .departure .points )} points"
172+ )
168173
169174 # remove effort values because scaled_joint_trajectory_controller does not support it.
170175 for pt in response .approach .points :
@@ -174,7 +179,7 @@ def main(args=None):
174179 for pt in response .departure .points :
175180 pt .effort = []
176181
177- client .get_logger ().info ("\n [exec] Starting Execution..." )
182+ client .get_logger ().info ("[exec] Starting Execution..." )
178183
179184 # Synchronus control
180185 approach_future = client .request_control (response .approach )
0 commit comments