Skip to content

Commit

Permalink
CI対応
Browse files Browse the repository at this point in the history
  • Loading branch information
mizonon committed Nov 1, 2024
1 parent 1e1c7e2 commit 1a5fd93
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 43 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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の初期化
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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):
Expand Down
4 changes: 1 addition & 3 deletions crane_plus_examples_py/crane_plus_examples_py/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
70 changes: 35 additions & 35 deletions crane_plus_examples_py/launch/example.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()],
)

Expand Down

0 comments on commit 1a5fd93

Please sign in to comment.