カメラサンプル動作確認用の修正

rt-net · Nov 11, 2024 · c823afa · c823afa
1 parent 1ff1b3b
commit c823afa
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 16 deletions.
diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py
@@ -15,7 +15,7 @@
 import datetime
 import math
 
-from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
 
 # moveit python library
 from moveit.core.robot_state import RobotState
@@ -117,6 +117,7 @@ def __init__(self, crane_plus):
 
     def on_timer(self):
         # target_0のtf位置姿勢を取得
+        tf_msg = TransformStamped()
         try:
             tf_msg = self.tf_buffer.lookup_transform(
                 'base_link', 'target_0', Time())
@@ -219,16 +220,18 @@ def _control_gripper(self, angle):
 
     # アーム制御
     def _control_arm(self, x, y, z, roll, pitch, yaw):
-        target_pose = Pose()
-        target_pose.position.x = x
-        target_pose.position.y = y
-        target_pose.position.z = z
+        # target_pose = Pose()
+        target_pose = PoseStamped()
+        target_pose.header.frame_id = 'crane_plus_base'
+        target_pose.pose.position.x = x
+        target_pose.pose.position.y = y
+        target_pose.pose.position.z = z
         q = euler_to_quaternion(math.radians(roll), math.radians(pitch),
                                 math.radians(yaw))
-        target_pose.orientation.x = q[0]
-        target_pose.orientation.y = q[1]
-        target_pose.orientation.z = q[2]
-        target_pose.orientation.w = q[3]
+        target_pose.pose.orientation.x = q[0]
+        target_pose.pose.orientation.y = q[1]
+        target_pose.pose.orientation.z = q[2]
+        target_pose.pose.orientation.w = q[3]
         self.crane_plus_arm.set_start_state_to_current_state()
         self.crane_plus_arm.set_goal_state(
             pose_stamped_msg=target_pose, pose_link='crane_plus_link4'

diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py
@@ -15,6 +15,7 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
+from crane_plus_description.robot_description_loader import RobotDescriptionLoader
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
@@ -23,8 +24,7 @@
 
 
 def generate_launch_description():
-    ld = LaunchDescription()
-
+
     # declare_loaded_description = DeclareLaunchArgument(
     #     'loaded_description',
     #     default_value='',
@@ -33,6 +33,16 @@ def generate_launch_description():
     #                     crane_plus_description.',
     # )
 
+    ld = LaunchDescription()
+    description_loader = RobotDescriptionLoader()
+    ld.add_action(
+        DeclareLaunchArgument(
+            'loaded_description',
+            default_value=description_loader.load(),
+            description='Set robot_description text.  \
+                      It is recommended to use RobotDescriptionLoader() in crane_plus_description.',
+        )
+    )
     # ld.add_action(declare_loaded_description)
 
     moveit_config = (
@@ -79,7 +89,7 @@ def generate_launch_description():
     declare_example_name = DeclareLaunchArgument(
         'example', default_value='color_detection',
         description=('Set an example executable name: '
-                     '[color_detection]')
+                     '[aruco_detection, color_detection]')
     )
 
     picking_node = Node(
@@ -88,17 +98,19 @@ def generate_launch_description():
         executable='pick_and_place_tf',
         output='screen',
         parameters=[
-            moveit_config.robot_description,
-            moveit_config.robot_description_semantic,
-            moveit_config.robot_description_kinematics,
+            # moveit_config.robot_description,
+            # moveit_config.robot_description_semantic,
+            # moveit_config.robot_description_kinematics,
+            moveit_config.to_dict()
         ],
     )
 
     example_node = Node(
         name=[LaunchConfiguration('example'), '_node'],
         package='crane_plus_examples_py',
         executable=LaunchConfiguration('example'),
-        output='screen'
+        output='screen',
+        parameters=[moveit_config.to_dict()],
     )
 
     # ld = LaunchDescription()