From 1a5fd93fa121709831533dd2b3cab849d928802c Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 1 Nov 2024 12:19:15 +0900 Subject: [PATCH] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 4 +- .../pick_and_place_tf.py | 5 +- .../crane_plus_examples_py/utils.py | 4 +- .../launch/example.launch.py | 70 +++++++++---------- 4 files changed, 40 insertions(+), 43 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index dcf4e68..8dbe590 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import plan_and_execute - import math # moveit python library @@ -25,6 +23,8 @@ import rclpy from rclpy.logging import get_logger +from crane_plus_examples_py.utils import plan_and_execute + def main(args=None): # ros2の初期化 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 index 8ca5a77..b07d501 100644 --- 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 @@ -13,9 +13,6 @@ # limitations under the License. import datetime - -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - import math from geometry_msgs.msg import Pose @@ -38,6 +35,8 @@ from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + class PickAndPlaceTf(Node): def __init__(self, crane_plus): diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 7c0089e..48a2257 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -13,13 +13,11 @@ # limitations under the License. import time - import numpy as np +import quaternion from rclpy.logging import get_logger -import quaternion - # plan and execute def plan_and_execute( diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index e533965..70e675e 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -26,87 +26,87 @@ def generate_launch_description(): # declare_loaded_description = DeclareLaunchArgument( - # "loaded_description", - # default_value="", - # description="Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + # "loaded_description', + # default_value='', + # description='Set robot_description text. \ + # It is recommended to use RobotDescriptionLoader() in crane_plus_description.', # ) ld = LaunchDescription() description_loader = RobotDescriptionLoader() ld.add_action( DeclareLaunchArgument( - "loaded_description", + 'loaded_description', default_value=description_loader.load(), - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.', ) ) moveit_config = ( - MoveItConfigsBuilder("crane_plus") + MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) .robot_description( file_path=os.path.join( - get_package_share_directory("crane_plus_description"), - "urdf", - "crane_plus.urdf.xacro", + get_package_share_directory('crane_plus_description'), + 'urdf', + 'crane_plus.urdf.xacro', ), mappings={}, ) .robot_description_kinematics( - file_path=get_package_share_directory("crane_plus_moveit_config") - + "/config/kinematics.yaml" + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/kinematics.yaml' ) # .robot_description_semantic( - # file_path="config/crsane_plus.srdf", - # mappings={"model": "crane_plus"}, + # file_path='config/crsane_plus.srdf', + # mappings={'model': 'crane_plus'}, # ) - # .joint_limits(file_path="config/joint_limits.yaml") + # .joint_limits(file_path='config/joint_limits.yaml') .trajectory_execution( - file_path=get_package_share_directory("crane_plus_moveit_config") - + "/config/controllers.yaml" + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/controllers.yaml' ) - # .planning_pipelines(pipelines=["ompl"]) + # .planning_pipelines(pipelines=['ompl']) .moveit_cpp( - file_path=get_package_share_directory("crane_plus_examples_py") - + "/config/crane_plus_moveit_py_examples.yaml" + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' ) .to_moveit_configs() ) moveit_config.robot_description = { - "robot_description": LaunchConfiguration("loaded_description") + 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {"capabilities": ""} + moveit_config.move_group_capabilities = {'capabilities': ''} declare_example_name = DeclareLaunchArgument( - "example", - default_value="gripper_control", + 'example', + default_value='gripper_control', description=( - "Set an example executable name: " - "[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]" + 'Set an example executable name: ' + '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' ), ) # declare_use_sim_time = DeclareLaunchArgument( - # "use_sim_time", - # default_value="false", - # description=("Set true when using the gazebo simulator."), + # 'use_sim_time', + # default_value='false', + # description=('Set true when using the gazebo simulator.'), # ) # use_sim_time_name = SetParameter( - # name="use_sim_time", value=LaunchConfiguration("use_sim_time") + # name='use_sim_time', value=LaunchConfiguration('use_sim_time') # ) example_node = Node( - name=[LaunchConfiguration("example"), "_node"], - package="crane_plus_examples_py", - executable=LaunchConfiguration("example"), - output="screen", + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', parameters=[moveit_config.to_dict()], )