Skip to content

Commit 4e18569

Browse files
brettpacbrettpac
committed
Refactored cl_moveit2z to a pure component based architecture
1 parent 702c66a commit 4e18569

File tree

10 files changed

+2078
-270
lines changed

10 files changed

+2078
-270
lines changed

‎smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp‎

Lines changed: 42 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include <tf2/transform_datatypes.h>
2323
#include <tf2_ros/transform_listener.h>
24+
#include <cl_moveit2z/components/cp_tf_listener.hpp>
2425
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
2526
#include "cb_move_end_effector_trajectory.hpp"
2627

@@ -225,12 +226,10 @@ class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory
225226
private:
226227
void computeCurrentEndEffectorPoseRelativeToPivot()
227228
{
228-
//auto currentRobotEndEffectorPose = this->movegroupClient_->moveGroupClientInterface.getCurrentPose();
229+
// Use CpTfListener component for transform lookups
230+
CpTfListener * tfListener = nullptr;
231+
this->requiresComponent(tfListener, false); // Optional component
229232

230-
tf2_ros::Buffer tfBuffer(getNode()->get_clock());
231-
tf2_ros::TransformListener tfListener(tfBuffer);
232-
233-
// tf2::Stamped<tf2::Transform> globalBaseLink;
234233
tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
235234

236235
try
@@ -243,20 +242,48 @@ class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory
243242
RCLCPP_INFO_STREAM(
244243
getLogger(), "[" << getName() << "] waiting transform, pivot: '"
245244
<< planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'");
246-
tf2::fromMsg(
247-
tfBuffer.lookupTransform(
248-
planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
249-
endEffectorInPivotFrame);
250-
251-
//endEffectorInPivotFrame = tfBuffer.lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(0));
252245

253-
// we define here the global frame as the pivot frame id
254-
// tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10));
255-
// tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink);
246+
if (tfListener != nullptr)
247+
{
248+
// Use component-based TF listener (preferred)
249+
auto transformOpt =
250+
tfListener->lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time());
251+
252+
if (transformOpt)
253+
{
254+
tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame);
255+
}
256+
else
257+
{
258+
RCLCPP_ERROR_STREAM(
259+
getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
260+
<< " to " << planePivotPose_.header.frame_id);
261+
return;
262+
}
263+
}
264+
else
265+
{
266+
// Fallback to legacy TF2 usage if component not available
267+
RCLCPP_WARN_STREAM(
268+
getLogger(), "[" << getName()
269+
<< "] CpTfListener component not available, using legacy TF2 (consider "
270+
"adding CpTfListener component)");
271+
tf2_ros::Buffer tfBuffer(getNode()->get_clock());
272+
tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
273+
274+
tf2::fromMsg(
275+
tfBuffer.lookupTransform(
276+
planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
277+
endEffectorInPivotFrame);
278+
}
256279
}
257280
catch (const std::exception & e)
258281
{
259-
std::cerr << e.what() << '\n';
282+
RCLCPP_ERROR_STREAM(
283+
getLogger(),
284+
"[" << getName()
285+
<< "] Exception in computeCurrentEndEffectorPoseRelativeToPivot: " << e.what());
286+
return;
260287
}
261288

262289
// tf2::Transform endEffectorInBaseLinkFrame;

‎smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp‎

Lines changed: 51 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <tf2_ros/transform_listener.h>
2323
#include <ament_index_cpp/get_package_share_directory.hpp>
2424
#include <cl_moveit2z/common.hpp>
25+
#include <cl_moveit2z/components/cp_tf_listener.hpp>
2526
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
2627
#include "cb_circular_pivot_motion.hpp"
2728

@@ -43,9 +44,9 @@ class CbEndEffectorRotate : public CbCircularPivotMotion
4344

4445
virtual void onEntry() override
4546
{
46-
// autocompute pivot pose
47-
tf2_ros::Buffer tfBuffer(getNode()->get_clock());
48-
tf2_ros::TransformListener tfListener(tfBuffer);
47+
// Use CpTfListener component for transform lookups
48+
CpTfListener * tfListener = nullptr;
49+
this->requiresComponent(tfListener, false); // Optional component
4950

5051
tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
5152

@@ -64,27 +65,60 @@ class CbEndEffectorRotate : public CbCircularPivotMotion
6465
{
6566
try
6667
{
67-
//auto pivotFrameName = this->movegroupClient_->moveGroupClientInterface->getPlanningFrame();
6868
auto pivotFrameName =
6969
this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
7070

71-
tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
72-
73-
tf2::fromMsg(
74-
tfBuffer.lookupTransform(
75-
pivotFrameName, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
76-
endEffectorInPivotFrame);
77-
78-
tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose);
79-
this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_;
80-
this->planePivotPose_.header.stamp =
81-
rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count());
82-
break;
71+
if (tfListener != nullptr)
72+
{
73+
// Use component-based TF listener (preferred)
74+
auto transformOpt =
75+
tfListener->lookupTransform(pivotFrameName, *tipLink_, rclcpp::Time());
76+
77+
if (transformOpt)
78+
{
79+
tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame);
80+
tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose);
81+
this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_;
82+
this->planePivotPose_.header.stamp =
83+
rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count());
84+
break;
85+
}
86+
else
87+
{
88+
RCLCPP_ERROR_STREAM(
89+
getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
90+
<< " to " << pivotFrameName << ". Attempt countdown: " << attempts);
91+
rclcpp::sleep_for(500ms);
92+
attempts--;
93+
}
94+
}
95+
else
96+
{
97+
// Fallback to legacy TF2 usage if component not available
98+
RCLCPP_WARN_STREAM(
99+
getLogger(),
100+
"[" << getName()
101+
<< "] CpTfListener component not available, using legacy TF2 (consider "
102+
"adding CpTfListener component)");
103+
tf2_ros::Buffer tfBuffer(getNode()->get_clock());
104+
tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
105+
106+
tf2::fromMsg(
107+
tfBuffer.lookupTransform(
108+
pivotFrameName, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
109+
endEffectorInPivotFrame);
110+
111+
tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose);
112+
this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_;
113+
this->planePivotPose_.header.stamp =
114+
rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count());
115+
break;
116+
}
83117
}
84118
catch (const std::exception & e)
85119
{
86120
RCLCPP_ERROR_STREAM(getLogger(), e.what() << ". Attempt countdown: " << attempts);
87-
rclcpp::Duration(500ms);
121+
rclcpp::sleep_for(500ms);
88122
attempts--;
89123
}
90124
}

‎smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector.hpp‎

Lines changed: 106 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,8 @@
2323
#include <tf2/impl/utils.h>
2424
#include <cl_moveit2z/cl_moveit2z.hpp>
2525
#include <cl_moveit2z/common.hpp>
26+
#include <cl_moveit2z/components/cp_motion_planner.hpp>
27+
#include <cl_moveit2z/components/cp_trajectory_executor.hpp>
2628
#include <future>
2729
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
2830
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -77,41 +79,130 @@ class CbMoveEndEffector : public smacc2::SmaccAsyncClientBehavior
7779
RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds");
7880
rclcpp::sleep_for(500ms);
7981

80-
moveGroupInterface.setPlanningTime(1.0);
82+
// Try to use CpMotionPlanner component (preferred)
83+
CpMotionPlanner * motionPlanner = nullptr;
84+
this->requiresComponent(motionPlanner, false); // Optional component
85+
86+
bool success = false;
87+
moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
8188

8289
RCLCPP_INFO_STREAM(
83-
getLogger(), "[CbMoveEndEffector] Target End efector Pose: " << targetObjectPose);
90+
getLogger(), "[CbMoveEndEffector] Target End effector Pose: " << targetObjectPose);
8491

85-
moveGroupInterface.setPoseTarget(targetObjectPose, tip_link_);
86-
moveGroupInterface.setPoseReferenceFrame(targetObjectPose.header.frame_id);
92+
if (motionPlanner != nullptr)
93+
{
94+
// Use component-based motion planner (preferred)
95+
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] Using CpMotionPlanner component for planning");
8796

88-
moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
89-
bool success =
90-
(moveGroupInterface.plan(computedMotionPlan) == moveit::core::MoveItErrorCode::SUCCESS);
91-
RCLCPP_INFO(
92-
getLogger(), "[CbMoveEndEffector] Success Visualizing plan 1 (pose goal) %s",
93-
success ? "" : "FAILED");
97+
PlanningOptions options;
98+
options.planningTime = 1.0;
99+
options.poseReferenceFrame = targetObjectPose.header.frame_id;
100+
101+
std::optional<std::string> tipLinkOpt;
102+
if (!tip_link_.empty())
103+
{
104+
tipLinkOpt = tip_link_;
105+
}
94106

107+
auto result = motionPlanner->planToPose(targetObjectPose, tipLinkOpt, options);
108+
109+
success = result.success;
110+
if (success)
111+
{
112+
computedMotionPlan = result.plan;
113+
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] Planning succeeded (via CpMotionPlanner)");
114+
}
115+
else
116+
{
117+
RCLCPP_WARN(
118+
getLogger(), "[CbMoveEndEffector] Planning failed (via CpMotionPlanner): %s",
119+
result.errorMessage.c_str());
120+
}
121+
}
122+
else
123+
{
124+
// Fallback to legacy direct API calls
125+
RCLCPP_WARN(
126+
getLogger(),
127+
"[CbMoveEndEffector] CpMotionPlanner component not available, using legacy planning "
128+
"(consider adding CpMotionPlanner component)");
129+
130+
moveGroupInterface.setPlanningTime(1.0);
131+
moveGroupInterface.setPoseTarget(targetObjectPose, tip_link_);
132+
moveGroupInterface.setPoseReferenceFrame(targetObjectPose.header.frame_id);
133+
134+
success =
135+
(moveGroupInterface.plan(computedMotionPlan) == moveit::core::MoveItErrorCode::SUCCESS);
136+
RCLCPP_INFO(
137+
getLogger(), "[CbMoveEndEffector] Planning %s (legacy mode)",
138+
success ? "succeeded" : "FAILED");
139+
}
140+
141+
// Execution
95142
if (success)
96143
{
97-
auto executionResult = moveGroupInterface.execute(computedMotionPlan);
144+
// Try to use CpTrajectoryExecutor component (preferred)
145+
CpTrajectoryExecutor * trajectoryExecutor = nullptr;
146+
this->requiresComponent(trajectoryExecutor, false); // Optional component
147+
148+
bool executionSuccess = false;
149+
150+
if (trajectoryExecutor != nullptr)
151+
{
152+
// Use component-based trajectory executor (preferred)
153+
RCLCPP_INFO(
154+
getLogger(), "[CbMoveEndEffector] Using CpTrajectoryExecutor component for execution");
155+
156+
ExecutionOptions execOptions;
157+
execOptions.trajectoryName = this->getName();
158+
159+
auto execResult = trajectoryExecutor->executePlan(computedMotionPlan, execOptions);
160+
executionSuccess = execResult.success;
161+
162+
if (executionSuccess)
163+
{
164+
RCLCPP_INFO(
165+
getLogger(), "[CbMoveEndEffector] Execution succeeded (via CpTrajectoryExecutor)");
166+
}
167+
else
168+
{
169+
RCLCPP_WARN(
170+
getLogger(), "[CbMoveEndEffector] Execution failed (via CpTrajectoryExecutor): %s",
171+
execResult.errorMessage.c_str());
172+
}
173+
}
174+
else
175+
{
176+
// Fallback to legacy direct execution
177+
RCLCPP_WARN(
178+
getLogger(),
179+
"[CbMoveEndEffector] CpTrajectoryExecutor component not available, using legacy "
180+
"execution "
181+
"(consider adding CpTrajectoryExecutor component)");
182+
183+
auto executionResult = moveGroupInterface.execute(computedMotionPlan);
184+
executionSuccess = (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
185+
186+
RCLCPP_INFO(
187+
getLogger(), "[CbMoveEndEffector] Execution %s (legacy mode)",
188+
executionSuccess ? "succeeded" : "failed");
189+
}
98190

99-
if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
191+
// Post events
192+
if (executionSuccess)
100193
{
101-
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution succeeded");
102194
movegroupClient_->postEventMotionExecutionSucceded();
103195
this->postSuccessEvent();
104196
}
105197
else
106198
{
107-
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed");
108199
movegroupClient_->postEventMotionExecutionFailed();
109200
this->postFailureEvent();
110201
}
111202
}
112203
else
113204
{
114-
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed");
205+
RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] planning failed, skipping execution");
115206
movegroupClient_->postEventMotionExecutionFailed();
116207
this->postFailureEvent();
117208
}

0 commit comments

Comments
 (0)