Add motion planning actions and services for iiwa robot
- Implemented action servers for MoveToPose and MoveToJoints. - Added service for MoveToNamedPose and stop action. - Updated README with usage examples for new actions. - Enhanced planning configurations in YAML and Python files. - Removed deprecated motion planning scripts.
This commit is contained in:
Даниил Грабарь
@@ -37,13 +37,6 @@ install(TARGETS
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/motion_planning_test.py
|
||||
# scripts/motion_planning.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
RENAME motion_planning
|
||||
)
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/move_to_pose_server.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
|
||||
@@ -1,129 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from moveit.core.robot_state import RobotState
|
||||
from moveit.planning import MoveItPy, PlanningComponent
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from tf_transformations import quaternion_from_euler
|
||||
|
||||
|
||||
class MotionPlaning(Node):
|
||||
def __init__(self):
|
||||
super().__init__("motion_planning_node")
|
||||
|
||||
self.moveit = MoveItPy(node_name="motion_planning_node")
|
||||
self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
|
||||
self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
|
||||
|
||||
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().info("MotionPlanning node is ready...")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
# self.robot_arm.set_start_state(configuration_name="home")
|
||||
# self.robot_arm.set_goal_state(configuration_name="work")
|
||||
# self.plan_end_execute()
|
||||
|
||||
self.timer = self.create_timer(5.0, self._run_once)
|
||||
|
||||
def _run_once(self):
|
||||
self.timer.cancel()
|
||||
self.handle_plan_execute_pose(0.677, 0.25, 0.21, 3.14, 0, 3.14)
|
||||
|
||||
|
||||
def handle_plan_execute_pose(self, x, y, z, a, b, c):
|
||||
|
||||
pose_goal = PoseStamped()
|
||||
pose_goal.header.frame_id="base_link"
|
||||
pose_goal.header.stamp = self.get_clock().now().to_msg()
|
||||
pose_goal.pose.position.x=x
|
||||
pose_goal.pose.position.y=y
|
||||
pose_goal.pose.position.z=z
|
||||
|
||||
xx, yy, zz, w = quaternion_from_euler(a, b, c)
|
||||
|
||||
pose_goal.pose.orientation.x=xx
|
||||
pose_goal.pose.orientation.y=yy
|
||||
pose_goal.pose.orientation.z=zz
|
||||
pose_goal.pose.orientation.w=w
|
||||
|
||||
# Check collisions
|
||||
with self.planning_scene_monitor.read_only() as scene:
|
||||
|
||||
robot_state = scene.current_state
|
||||
original_joint_positions = robot_state.get_joint_group_positions("iiwa_arm")
|
||||
|
||||
ok_ik = robot_state.set_from_ik("iiwa_arm", pose_goal.pose, "link_ee")
|
||||
robot_state.update()
|
||||
|
||||
if not ok_ik:
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().error("IK failed -> abort")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
return
|
||||
|
||||
robot_collision_status = scene.is_state_colliding(
|
||||
robot_state=robot_state,
|
||||
joint_model_group_name="iiwa_arm",
|
||||
verbose=True
|
||||
)
|
||||
if robot_collision_status:
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().error("Goal in collision -> abort")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
return
|
||||
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().info(f"\nRobot is in collision: {robot_collision_status}\n")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
|
||||
robot_state.set_joint_group_positions(
|
||||
"iiwa_arm",
|
||||
original_joint_positions,
|
||||
)
|
||||
robot_state.update()
|
||||
|
||||
# получить текущее состояние как изначальное для перемещения
|
||||
self.robot_arm.set_start_state_to_current_state()
|
||||
# self.robot_arm.set_start_state(configuration_name="home")
|
||||
# self.robot_arm.set_goal_state(configuration_name="work")
|
||||
|
||||
self.robot_arm.set_goal_state(
|
||||
pose_stamped_msg=pose_goal,
|
||||
pose_link="link_ee"
|
||||
)
|
||||
self.plan_end_execute()
|
||||
|
||||
|
||||
def plan_end_execute(self):
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().info("Planning trajectory")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
|
||||
plan_result = self.robot_arm.plan()
|
||||
if plan_result:
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().info("Executing plan")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
robot_trajectory = plan_result.trajectory
|
||||
self.moveit.execute(robot_trajectory, controllers=[])
|
||||
else:
|
||||
self.get_logger().warn("***********************************************************")
|
||||
self.get_logger().info("Planning failed")
|
||||
self.get_logger().warn("***********************************************************")
|
||||
|
||||
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = MotionPlaning()
|
||||
rclpy.spin(node=node)
|
||||
node.destroy_node()
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,89 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
|
||||
# generic ros libraries
|
||||
import rclpy
|
||||
from moveit.core.robot_state import RobotState
|
||||
from moveit.planning import MoveItPy
|
||||
from rclpy.impl.rcutils_logger import RcutilsLogger
|
||||
from rclpy.logging import get_logger
|
||||
|
||||
|
||||
def plan_and_execute(robot: MoveItPy,
|
||||
planning_component,
|
||||
logger: RcutilsLogger,
|
||||
single_plan_parameters=None,
|
||||
multi_plan_parameters=None,
|
||||
sleep_time=0.0):
|
||||
logger.info("Planning trajectory")
|
||||
|
||||
if multi_plan_parameters is not None:
|
||||
plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters)
|
||||
elif single_plan_parameters is not None:
|
||||
plan_result = planning_component.plan(
|
||||
single_plan_parameters=single_plan_parameters
|
||||
)
|
||||
else:
|
||||
plan_result = planning_component.plan()
|
||||
|
||||
# execute the plan
|
||||
if plan_result:
|
||||
logger.info("Executing plan")
|
||||
robot_trajectory = plan_result.trajectory
|
||||
robot.execute(robot_trajectory, controllers=[])
|
||||
else:
|
||||
logger.error("Planning failed")
|
||||
|
||||
time.sleep(sleep_time)
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
logger = get_logger("moveit_py.pose_goal")
|
||||
|
||||
iiwa = MoveItPy(node_name="moveit_py")
|
||||
iiwa_arm = iiwa.get_planning_component("iiwa_arm")
|
||||
logger.info("MoveItPy instance created")
|
||||
|
||||
iiwa_arm.set_start_state(configuration_name="ready")
|
||||
iiwa_arm.set_goal_state(configuration_name="extended")
|
||||
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
robot_model = iiwa.get_robot_model()
|
||||
robot_state = RobotState(robot_model)
|
||||
|
||||
# randomize the robot state
|
||||
robot_state.set_to_random_positions()
|
||||
|
||||
# set plan start state to current state
|
||||
iiwa_arm.set_start_state_to_current_state()
|
||||
|
||||
# set goal state to the initialized robot state
|
||||
logger.info("Set goal state to the initialized robot state")
|
||||
iiwa_arm.set_goal_state(robot_state=robot_state)
|
||||
|
||||
# plan to goal
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
# set plan start state to current state
|
||||
iiwa_arm.set_start_state_to_current_state()
|
||||
|
||||
# set pose goal with PoseStamped message
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
pose_goal = PoseStamped()
|
||||
pose_goal.header.frame_id = "base_link"
|
||||
pose_goal.pose.orientation.w = 1.0
|
||||
pose_goal.pose.position.x = 0.28
|
||||
pose_goal.pose.position.y = -0.2
|
||||
pose_goal.pose.position.z = 0.5
|
||||
iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal,
|
||||
pose_link="patron")
|
||||
|
||||
# plan to goal
|
||||
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,67 +1,290 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import threading
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.action import ActionServer, CancelResponse, GoalResponse
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException
|
||||
from moveit.planning import MoveItPy, PlanningComponent
|
||||
from iiwa_msgs.srv import MoveToPose
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from scipy.spatial.transform import Rotation
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from moveit.planning import MoveItPy, PlanningComponent, PlanRequestParameters
|
||||
from moveit.core.robot_state import RobotState
|
||||
|
||||
from iiwa_msgs.action import MoveToPose, MoveToJoints
|
||||
from iiwa_msgs.srv import MoveToNamedPose
|
||||
|
||||
|
||||
POSE_LINK = "link_ee"
|
||||
PLANNING_GROUP = "iiwa_arm"
|
||||
# Таблица планировщиков: имя → (pipeline, planner_id, время планирования в секундах)
|
||||
PLANNERS = {
|
||||
"ompl": ("ompl", "RRTConnectkConfigDefault", 10.0),
|
||||
"ptp": ("pilz_industrial_motion_planner", "PTP", 2.0),
|
||||
"lin": ("pilz_industrial_motion_planner", "LIN", 2.0),
|
||||
"circ": ("pilz_industrial_motion_planner", "CIRC", 2.0),
|
||||
"chomp": ("chomp", "", 10.0),
|
||||
}
|
||||
|
||||
|
||||
def _abc_to_quaternion(a: float, b: float, c: float):
|
||||
"""ZYX Euler (радианы, конвенция KUKA ABC) → (qx, qy, qz, qw)."""
|
||||
return Rotation.from_euler("ZYX", [a, b, c], degrees=False).as_quat()
|
||||
|
||||
|
||||
class IiwaMotionServer(Node):
|
||||
"""Сервер управления движением манипулятора iiwa.
|
||||
|
||||
Предоставляет:
|
||||
- action iiwa/move_to_pose — перемещение в декартову позу
|
||||
- action iiwa/move_to_joints — перемещение по суставным координатам
|
||||
- service iiwa/move_to_named — перемещение в именованную позу из SRDF
|
||||
- service iiwa/stop — немедленная остановка движения
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("iiwa_motion_server")
|
||||
self._setup_parameters()
|
||||
self._setup_moveit()
|
||||
self._setup_servers()
|
||||
|
||||
self.get_logger().info("Сервер движения iiwa запущен")
|
||||
self.get_logger().info(f" pose_link={self._pose_link} group={self._planning_group}")
|
||||
|
||||
def _setup_parameters(self):
|
||||
self.declare_parameter("pose_link", "link_ee")
|
||||
self.declare_parameter("planning_group", "iiwa_arm")
|
||||
self.declare_parameter("default_frame", "base_link")
|
||||
self.declare_parameter("default_planner", "ompl")
|
||||
self.declare_parameter("planning_attempts", 3)
|
||||
|
||||
self._pose_link = self.get_parameter("pose_link").value
|
||||
self._planning_group = self.get_parameter("planning_group").value
|
||||
self._default_frame = self.get_parameter("default_frame").value
|
||||
self._default_planner = self.get_parameter("default_planner").value
|
||||
self._planning_attempts = self.get_parameter("planning_attempts").value
|
||||
|
||||
def _setup_moveit(self):
|
||||
self._moveit = MoveItPy(node_name="iiwa_motion_server")
|
||||
self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
|
||||
self._robot_model = self._moveit.get_robot_model()
|
||||
|
||||
def _setup_servers(self):
|
||||
cb = ReentrantCallbackGroup()
|
||||
|
||||
ActionServer(
|
||||
self, MoveToPose, "iiwa/move_to_pose", self._execute_pose,
|
||||
callback_group=cb,
|
||||
goal_callback=lambda _: GoalResponse.ACCEPT,
|
||||
cancel_callback=lambda _: CancelResponse.ACCEPT,
|
||||
)
|
||||
ActionServer(
|
||||
self, MoveToJoints, "iiwa/move_to_joints", self._execute_joints,
|
||||
callback_group=cb,
|
||||
goal_callback=lambda _: GoalResponse.ACCEPT,
|
||||
cancel_callback=lambda _: CancelResponse.ACCEPT,
|
||||
)
|
||||
self.create_service(MoveToNamedPose, "iiwa/move_to_named", self._handle_named, callback_group=cb)
|
||||
self.create_service(Trigger, "iiwa/stop", self._handle_stop, callback_group=cb)
|
||||
|
||||
def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float) -> PlanRequestParameters:
|
||||
params = PlanRequestParameters(self._moveit, self._planning_group)
|
||||
params.planning_pipeline = pipeline
|
||||
params.planner_id = planner_id
|
||||
params.planning_time = plan_time
|
||||
params.planning_attempts = self._planning_attempts
|
||||
params.max_velocity_scaling_factor = velocity_scale
|
||||
params.max_acceleration_scaling_factor = velocity_scale
|
||||
return params
|
||||
|
||||
def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):
|
||||
"""Планирует траекторию и выполняет её с поддержкой отмены.
|
||||
|
||||
Возвращает (True, msg) при успехе, (False, msg) при ошибке,
|
||||
(None, 'canceled') если цель была отменена.
|
||||
"""
|
||||
plan_result = self._arm.plan(single_plan_parameters=plan_params)
|
||||
if not plan_result:
|
||||
return False, "Планирование не удалось: поза недостижима или в столкновении"
|
||||
|
||||
if goal_handle.is_cancel_requested:
|
||||
return None, "canceled"
|
||||
|
||||
done = threading.Event()
|
||||
failed = threading.Event()
|
||||
|
||||
def do_execute():
|
||||
try:
|
||||
self._moveit.execute(plan_result.trajectory, controllers=[])
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
|
||||
failed.set()
|
||||
finally:
|
||||
done.set()
|
||||
|
||||
threading.Thread(target=do_execute, daemon=True).start()
|
||||
|
||||
while not done.wait(timeout=0.05):
|
||||
if goal_handle.is_cancel_requested:
|
||||
try:
|
||||
self._moveit.get_trajectory_execution_manager().stop_execution()
|
||||
except Exception:
|
||||
pass
|
||||
done.wait()
|
||||
return None, "canceled"
|
||||
|
||||
if failed.is_set():
|
||||
return False, "Выполнение траектории завершилось ошибкой"
|
||||
|
||||
return True, "Движение выполнено успешно"
|
||||
|
||||
def _finish_action(self, goal_handle, result, ok, msg):
|
||||
"""Устанавливает финальное состояние action goal и заполняет result."""
|
||||
result.success = bool(ok)
|
||||
result.message = msg
|
||||
if ok is None:
|
||||
goal_handle.canceled()
|
||||
elif ok:
|
||||
goal_handle.succeed()
|
||||
else:
|
||||
self.get_logger().error(msg)
|
||||
goal_handle.abort()
|
||||
return result
|
||||
|
||||
def _execute_pose(self, goal_handle):
|
||||
req = goal_handle.request
|
||||
feedback = MoveToPose.Feedback()
|
||||
result = MoveToPose.Result()
|
||||
|
||||
velocity_scale = max(0.01, min(1.0, float(req.speed)))
|
||||
planner_key = (req.planner or self._default_planner).lower()
|
||||
|
||||
if planner_key not in PLANNERS:
|
||||
result.success = False
|
||||
result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
|
||||
self.get_logger().error(result.message)
|
||||
goal_handle.abort()
|
||||
return result
|
||||
|
||||
pipeline, planner_id, plan_time = PLANNERS[planner_key]
|
||||
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = req.frame_id or self._default_frame
|
||||
pose.pose.position.x = req.x
|
||||
pose.pose.position.y = req.y
|
||||
pose.pose.position.z = req.z
|
||||
qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
|
||||
pose.pose.orientation.x = qx
|
||||
pose.pose.orientation.y = qy
|
||||
pose.pose.orientation.z = qz
|
||||
pose.pose.orientation.w = qw
|
||||
|
||||
self.get_logger().info(
|
||||
f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f}) "
|
||||
f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад "
|
||||
f"speed={velocity_scale:.2f} planner={planner_key}"
|
||||
)
|
||||
|
||||
feedback.state = "planning"
|
||||
goal_handle.publish_feedback(feedback)
|
||||
|
||||
self._arm.set_start_state_to_current_state()
|
||||
self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
|
||||
|
||||
feedback.state = "executing"
|
||||
goal_handle.publish_feedback(feedback)
|
||||
|
||||
plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
|
||||
ok, msg = self._plan_and_execute(plan_params, goal_handle)
|
||||
return self._finish_action(goal_handle, result, ok, msg)
|
||||
|
||||
def _execute_joints(self, goal_handle):
|
||||
req = goal_handle.request
|
||||
feedback = MoveToJoints.Feedback()
|
||||
result = MoveToJoints.Result()
|
||||
|
||||
velocity_scale = max(0.01, min(1.0, float(req.speed)))
|
||||
joints = list(req.joints)
|
||||
|
||||
self.get_logger().info(
|
||||
f"[joints] {[f'{v:.3f}' for v in joints]} speed={velocity_scale:.2f}"
|
||||
)
|
||||
|
||||
feedback.state = "planning"
|
||||
goal_handle.publish_feedback(feedback)
|
||||
|
||||
# Формируем целевое состояние по суставным координатам
|
||||
goal_state = RobotState(self._robot_model)
|
||||
goal_state.set_joint_group_positions(self._planning_group, joints)
|
||||
goal_state.update()
|
||||
|
||||
self._arm.set_start_state_to_current_state()
|
||||
self._arm.set_goal_state(robot_state=goal_state)
|
||||
|
||||
feedback.state = "executing"
|
||||
goal_handle.publish_feedback(feedback)
|
||||
|
||||
plan_params = self._make_plan_params(
|
||||
"ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
|
||||
)
|
||||
ok, msg = self._plan_and_execute(plan_params, goal_handle)
|
||||
return self._finish_action(goal_handle, result, ok, msg)
|
||||
|
||||
def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
|
||||
name = request.name.strip()
|
||||
velocity_scale = max(0.01, min(1.0, float(request.speed)))
|
||||
|
||||
self.get_logger().info(f"[named] name='{name}' speed={velocity_scale:.2f}")
|
||||
|
||||
self._arm.set_start_state_to_current_state()
|
||||
try:
|
||||
self._arm.set_goal_state(configuration_name=name)
|
||||
except Exception as exc:
|
||||
response.success = False
|
||||
response.message = f"Неизвестное состояние '{name}': {exc}"
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
plan_params = self._make_plan_params(
|
||||
"ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
|
||||
)
|
||||
plan_result = self._arm.plan(single_plan_parameters=plan_params)
|
||||
if not plan_result:
|
||||
response.success = False
|
||||
response.message = f"Не удалось построить траекторию для '{name}'"
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
self._moveit.execute(plan_result.trajectory, controllers=[])
|
||||
response.success = True
|
||||
response.message = f"Переместился в '{name}'"
|
||||
self.get_logger().info(response.message)
|
||||
return response
|
||||
|
||||
def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
|
||||
try:
|
||||
self._moveit.get_trajectory_execution_manager().stop_execution()
|
||||
response.success = True
|
||||
response.message = "Выполнение траектории остановлено"
|
||||
except Exception as exc:
|
||||
response.success = False
|
||||
response.message = str(exc)
|
||||
self.get_logger().info(f"[stop] {response.message}")
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
# MoveItPy создаёт C++ узел с именем из лонча → читает robot_description_kinematics и т.д.
|
||||
moveit = MoveItPy(node_name="move_to_pose_server")
|
||||
arm: PlanningComponent = moveit.get_planning_component(PLANNING_GROUP)
|
||||
|
||||
# Отдельный лёгкий узел для сервиса — другое имя, нет конфликта параметров
|
||||
node = Node("move_to_pose_service")
|
||||
logger = node.get_logger()
|
||||
|
||||
cb_group = ReentrantCallbackGroup()
|
||||
|
||||
def handle(request: MoveToPose.Request, response: MoveToPose.Response):
|
||||
pose = request.pose
|
||||
if not pose.header.frame_id:
|
||||
pose.header.frame_id = "base_link"
|
||||
|
||||
arm.set_start_state_to_current_state()
|
||||
arm.set_goal_state(pose_stamped_msg=pose, pose_link=POSE_LINK)
|
||||
|
||||
logger.info(
|
||||
f"Planning to ({pose.pose.position.x:.3f}, "
|
||||
f"{pose.pose.position.y:.3f}, {pose.pose.position.z:.3f})"
|
||||
)
|
||||
plan_result = arm.plan()
|
||||
|
||||
if not plan_result:
|
||||
response.success = False
|
||||
response.message = "Planning failed: pose may be unreachable or in collision"
|
||||
logger.error(response.message)
|
||||
return response
|
||||
|
||||
moveit.execute(plan_result.trajectory, controllers=[])
|
||||
response.success = True
|
||||
response.message = "Motion executed successfully"
|
||||
logger.info(response.message)
|
||||
return response
|
||||
|
||||
node.create_service(MoveToPose, "iiwa/move_to_pose", handle, callback_group=cb_group)
|
||||
logger.info(f"MoveToPoseServer ready (pose_link={POSE_LINK})")
|
||||
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = IiwaMotionServer()
|
||||
executor = MultiThreadedExecutor()
|
||||
executor.add_node(node)
|
||||
executor.spin()
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user