diff --git a/smacc2/include/smacc2/smacc_signal_detector.hpp b/smacc2/include/smacc2/smacc_signal_detector.hpp index 8b90f1f93..de29d4b5e 100644 --- a/smacc2/include/smacc2/smacc_signal_detector.hpp +++ b/smacc2/include/smacc2/smacc_signal_detector.hpp @@ -53,6 +53,8 @@ class SignalDetector void stop(); + void terminateScheduler(); + void pollingLoop(); template @@ -105,6 +107,14 @@ class SignalDetector void onSigQuit(int sig); +// Global variables for graceful shutdown handling +// Declared here, defined in signal_detector.cpp +extern std::atomic g_shutdown_requested; +extern SignalDetector * g_signal_detector; + +// Signal handler for graceful shutdown (SIGINT/SIGTERM) +void onSignalShutdown(int sig); + // Main entry point for any SMACC state machine // It instantiates and starts the specified state machine type // it uses two threads: a new thread and the current one. @@ -113,6 +123,9 @@ void onSigQuit(int sig); template void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER) { + // Register signal handlers for graceful shutdown + ::signal(SIGINT, onSignalShutdown); + ::signal(SIGTERM, onSignalShutdown); ::signal(SIGQUIT, onSigQuit); // create the asynchronous state machine scheduler @@ -121,6 +134,9 @@ void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER) // create the signalDetector component SignalDetector signalDetector(&scheduler1, executionModel); + // Store global reference for signal handler access + g_signal_detector = &signalDetector; + // create the asynchronous state machine processor SmaccFifoScheduler::processor_handle sm = scheduler1.create_processor(&signalDetector); @@ -135,6 +151,19 @@ void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER) // use the main thread for the signal detector component (waiting actionclient requests) signalDetector.pollingLoop(); + + // After polling loop exits (due to shutdown), terminate the scheduler from main thread + // This is safe to call here (not from signal handler) as we're in the main thread context + RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Polling loop exited. Terminating scheduler..."); + signalDetector.terminateScheduler(); + + // Wait for scheduler thread to finish + RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Waiting for scheduler thread to join..."); + schedulerThread.join(); + RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Scheduler thread terminated. Shutdown complete."); + + // Clear global reference + g_signal_detector = nullptr; } struct SmExecution @@ -149,6 +178,9 @@ struct SmExecution template SmExecution * run_async() { + // Register signal handlers for graceful shutdown + ::signal(SIGINT, onSignalShutdown); + ::signal(SIGTERM, onSignalShutdown); ::signal(SIGQUIT, onSigQuit); SmExecution * ret = new SmExecution(); @@ -159,6 +191,9 @@ SmExecution * run_async() // create the signalDetector component ret->signalDetector = new SignalDetector(ret->scheduler1); + // Store global reference for signal handler access + g_signal_detector = ret->signalDetector; + // create the asynchronous state machine processor ret->sm = ret->scheduler1->create_processor(ret->signalDetector); diff --git a/smacc2/src/smacc2/signal_detector.cpp b/smacc2/src/smacc2/signal_detector.cpp index e38733eb8..c5aed6429 100644 --- a/smacc2/src/smacc2/signal_detector.cpp +++ b/smacc2/src/smacc2/signal_detector.cpp @@ -36,6 +36,11 @@ namespace smacc2 { using namespace std::chrono_literals; + +// Define global variables for graceful shutdown handling +std::atomic g_shutdown_requested{false}; +SignalDetector * g_signal_detector = nullptr; + /** ****************************************************************************************************************** * SignalDetector() @@ -218,6 +223,20 @@ void SignalDetector::join() { signalDetectorThread_.join(); } */ void SignalDetector::stop() { end_ = true; } +/** + ****************************************************************************************************************** + * terminateScheduler() + ****************************************************************************************************************** + */ +void SignalDetector::terminateScheduler() +{ + if (scheduler_) + { + RCLCPP_INFO(getLogger(), "[SignalDetector] Terminating scheduler..."); + scheduler_->terminate(); + } +} + /** ****************************************************************************************************************** * poll() @@ -367,6 +386,27 @@ void SignalDetector::pollingLoop() } } +void onSignalShutdown(int sig) +{ + // IMPORTANT: Signal handlers can only call async-signal-safe functions + // We must NOT call complex C++ methods here (like terminateScheduler) + // as they may use mutexes/condition variables which are not signal-safe + + // Set global shutdown flag (atomic operation - signal-safe) + g_shutdown_requested = true; + + // Stop the signal detector loop (atomic operation - signal-safe) + if (g_signal_detector) + { + g_signal_detector->stop(); + } + + // Trigger ROS2 shutdown (this handles its own signal safety) + rclcpp::shutdown(); + + // Note: Scheduler termination will happen from main thread after polling loop exits +} + void onSigQuit(int) { RCLCPP_INFO(rclcpp::get_logger("SMACC"), "SignalDetector: SIGQUIT received."); diff --git a/smacc2_client_library/cl_moveit2z/CMakeLists.txt b/smacc2_client_library/cl_moveit2z/CMakeLists.txt index 8a94de756..c836ef96c 100644 --- a/smacc2_client_library/cl_moveit2z/CMakeLists.txt +++ b/smacc2_client_library/cl_moveit2z/CMakeLists.txt @@ -15,8 +15,6 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -#set(YAML_CPP_LIBRARIES yaml-cpp) - set(dependencies moveit_ros_planning_interface tf2 @@ -28,35 +26,30 @@ yaml_cpp_vendor include_directories(include) -## Declare a C++ library +## Declare a C++ library (header-only implementations with compilation anchor) add_library(${PROJECT_NAME} - src/cl_moveit2z/cl_moveit2z.cpp - src/cl_moveit2z/common.cpp - src/cl_moveit2z/components/cp_trajectory_history.cpp - src/cl_moveit2z/components/cp_grasping_objects.cpp - - src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp - src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp - src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp - src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp - src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp - src/cl_moveit2z/client_behaviors/cb_move_joints.cpp - src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp - src/cl_moveit2z/client_behaviors/cb_detach_object.cpp - src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp - src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp - src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp - src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp - src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp - src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp - src/cl_moveit2z/client_behaviors/cb_attach_object.cpp - src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp + src/cl_moveit2z/cl_moveit2z.cpp # Compilation anchor stub ) +# All implementations are header-only (inline): +# src/cl_moveit2z/common.cpp # PHASE 1: NOW HEADER-ONLY (inline stream operators) +# src/cl_moveit2z/components/cp_trajectory_history.cpp # PHASE 2: NOW HEADER-ONLY (inline methods) +# src/cl_moveit2z/components/cp_grasping_objects.cpp # PHASE 3: NOW HEADER-ONLY (inline methods) + +# ALL CLIENT BEHAVIORS ARE NOW HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_move_joints.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp # HEADER-ONLY +# src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp # HEADER-ONLY + target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME} - ${dependencies}) + ${dependencies}) ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp index 03c00b661..fb32ece09 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp @@ -91,11 +91,25 @@ class ClMoveit2z : public smacc2::ISmaccClient virtual ~ClMoveit2z(); - void onInitialize() override; + inline void onInitialize() override + { + moveGroupClientInterface = + std::make_shared(getNode(), options_); + planningSceneInterface = std::make_shared( + options_.move_group_namespace_); + } - void postEventMotionExecutionSucceded(); + inline void postEventMotionExecutionSucceded() + { + RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Success Event"); + postEventMotionExecutionSucceded_(); + } - void postEventMotionExecutionFailed(); + inline void postEventMotionExecutionFailed() + { + RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Failure Event"); + postEventMotionExecutionFailed_(); + } template void onOrthogonalAllocation() @@ -125,7 +139,10 @@ class ClMoveit2z : public smacc2::ISmaccClient return this->getStateMachine()->createSignalConnection(onFailed_, callback, object); } - const moveit::planning_interface::MoveGroupInterface::Options & getOptions() const; + inline const moveit::planning_interface::MoveGroupInterface::Options & getOptions() const + { + return options_; + } private: std::function postEventMotionExecutionSucceded_; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp index c5dc2ec52..07f84be29 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp @@ -20,23 +20,17 @@ #pragma once // BASIC MANIPULATION BEHAVIORSz -#include "client_behaviors/cb_move_cartesian_relative.hpp" #include "client_behaviors/cb_move_end_effector.hpp" -#include "client_behaviors/cb_move_end_effector_relative.hpp" #include "client_behaviors/cb_move_joints.hpp" #include "client_behaviors/cb_move_known_state.hpp" -#include "client_behaviors/cb_move_named_target.hpp" // ADVANCED MANIPULATION BEHAVIORS #include "client_behaviors/cb_circular_pivot_motion.hpp" #include "client_behaviors/cb_end_effector_rotate.hpp" #include "client_behaviors/cb_move_cartesian_relative2.hpp" #include "client_behaviors/cb_move_end_effector_trajectory.hpp" -#include "client_behaviors/cb_pouring_motion.hpp" // HISTORY BASED BEHAVIRORS -#include "client_behaviors/cb_execute_last_trajectory.hpp" -#include "client_behaviors/cb_move_last_trajectory_initial_state.hpp" #include "client_behaviors/cb_undo_last_trajectory.hpp" // GRASPING BEHAVIORS diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp index a7e3d3db9..be9d635a8 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp @@ -21,23 +21,83 @@ #pragma once #include +#include +#include #include namespace cl_moveit2z { +/** + * @brief Client behavior that attaches a collision object to the robot gripper + * + * This behavior attaches a previously created grasping object to the robot's + * end effector. The object must exist in the CpGraspingComponent's graspingObjects + * map. Upon successful attachment, the object is added to the planning scene + * and attached to the gripper using the finger tip links. + */ class CbAttachObject : public smacc2::SmaccAsyncClientBehavior { public: - CbAttachObject(std::string targetObjectName); + /** + * @brief Constructor with object name + * @param targetObjectName Name of the object to attach (must exist in CpGraspingComponent) + */ + inline CbAttachObject(std::string targetObjectName) : targetObjectName_(targetObjectName) {} - CbAttachObject(); + /** + * @brief Default constructor + */ + inline CbAttachObject() {} - virtual void onEntry() override; + /** + * @brief Called when the behavior is entered + * + * Retrieves the target object from CpGraspingComponent, adds it to the planning + * scene, and attaches it to the gripper. Posts success or failure event based + * on whether the object was found. + */ + inline void onEntry() override + { + cl_moveit2z::ClMoveit2z * moveGroup; + this->requiresClient(moveGroup); - virtual void onExit() override; + cl_moveit2z::CpGraspingComponent * graspingComponent; + this->requiresComponent(graspingComponent); - std::string targetObjectName_; + moveit_msgs::msg::CollisionObject targetCollisionObject; + bool found = graspingComponent->getGraspingObject(targetObjectName_, targetCollisionObject); + + if (found) + { + RCLCPP_INFO_STREAM(getLogger(), "[CbAttachObject] Attaching object: " << targetObjectName_); + + targetCollisionObject.operation = moveit_msgs::msg::CollisionObject::ADD; + targetCollisionObject.header.stamp = getNode()->now(); + + moveGroup->planningSceneInterface->applyCollisionObject(targetCollisionObject); + graspingComponent->currentAttachedObjectName = targetObjectName_; -private: + moveGroup->moveGroupClientInterface->attachObject( + targetObjectName_, graspingComponent->gripperLink_, graspingComponent->fingerTipNames); + + this->postSuccessEvent(); + } + else + { + RCLCPP_ERROR_STREAM( + getLogger(), + "[CbAttachObject] Object not found in grasping component: " << targetObjectName_); + this->postFailureEvent(); + } + } + + /** + * @brief Called when the behavior is exited + */ + inline void onExit() override {} + + /// Name of the target object to attach + std::string targetObjectName_; }; + } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp index b73bf5f1c..801fecfd6 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp @@ -20,8 +20,13 @@ #pragma once #include +#include +#include +#include #include "cb_move_end_effector_trajectory.hpp" +using namespace std::chrono_literals; + namespace cl_moveit2z { // executes a circular trajectory of the tipLink around one axis defined @@ -35,20 +40,182 @@ class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory // if not specified it would be used the current robot pose std::optional relativeInitialPose_; - CbCircularPivotMotion(std::optional tipLink = std::nullopt); + CbCircularPivotMotion(std::optional tipLink = std::nullopt) + : CbMoveEndEffectorTrajectory(tipLink) + { + } CbCircularPivotMotion( const geometry_msgs::msg::PoseStamped & planePivotPose, double deltaRadians, - std::optional tipLink = std::nullopt); + std::optional tipLink = std::nullopt) + : CbMoveEndEffectorTrajectory(tipLink), + planePivotPose_(planePivotPose), + deltaRadians_(deltaRadians) + { + if (tipLink_) planePivotPose_.header.frame_id = *tipLink; + } CbCircularPivotMotion( const geometry_msgs::msg::PoseStamped & planePivotPose, const geometry_msgs::msg::Pose & relativeInitialPose, double deltaRadians, - std::optional tipLink = std::nullopt); + std::optional tipLink = std::nullopt) + : CbMoveEndEffectorTrajectory(tipLink), + relativeInitialPose_(relativeInitialPose), + planePivotPose_(planePivotPose), + deltaRadians_(deltaRadians) + { + } + + virtual void generateTrajectory() override + { + if (!relativeInitialPose_) + { + this->computeCurrentEndEffectorPoseRelativeToPivot(); + } + + // project offset into the xy-plane + // get the radius + double radius = sqrt( + relativeInitialPose_->position.z * relativeInitialPose_->position.z + + relativeInitialPose_->position.y * relativeInitialPose_->position.y); + double initialAngle = atan2(relativeInitialPose_->position.z, relativeInitialPose_->position.y); + + double totallineardist = fabs(radius * deltaRadians_); + double totalangulardist = fabs(deltaRadians_); + + // at least 1 sample per centimeter (average) + // at least 1 sample per ~1.1 degrees (average) + + const double RADS_PER_SAMPLE = 0.02; + const double METERS_PER_SAMPLE = 0.01; + + int totalSamplesCount = + std::max(totallineardist / METERS_PER_SAMPLE, totalangulardist / RADS_PER_SAMPLE); + + double linearSecondsPerSample; + double angularSecondsPerSamples; + double secondsPerSample; + + if (linearSpeed_m_s_) + { + linearSecondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_); + } + else + { + linearSecondsPerSample = std::numeric_limits::max(); + } + + if (angularSpeed_rad_s_) + { + angularSecondsPerSamples = RADS_PER_SAMPLE / (*angularSpeed_rad_s_); + } + else + { + angularSecondsPerSamples = std::numeric_limits::max(); + } + + if (!linearSpeed_m_s_ && !angularSpeed_rad_s_) + { + secondsPerSample = 0.5; + } + else + { + secondsPerSample = std::min(linearSecondsPerSample, angularSecondsPerSamples); + } + + double currentAngle = initialAngle; + + double angleStep = deltaRadians_ / (double)totalSamplesCount; + + tf2::Transform tfBasePose; + tf2::fromMsg(planePivotPose_.pose, tfBasePose); + + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] generated trajectory, total samples: " << totalSamplesCount); + for (int i = 0; i < totalSamplesCount; i++) + { + // relativePose i + double y = radius * cos(currentAngle); + double z = radius * sin(currentAngle); + + geometry_msgs::msg::Pose relativeCurrentPose; + + relativeCurrentPose.position.x = relativeInitialPose_->position.x; + relativeCurrentPose.position.y = y; + relativeCurrentPose.position.z = z; + + tf2::Quaternion localquat; + localquat.setEuler(currentAngle, 0, 0); + + //relativeCurrentPose.orientation = relativeInitialPose_.orientation; + //tf2::toMsg(localquat, relativeCurrentPose.orientation); + relativeCurrentPose.orientation.w = 1; + + tf2::Transform tfRelativeCurrentPose; + tf2::fromMsg(relativeCurrentPose, tfRelativeCurrentPose); + + tf2::Transform tfGlobalPose = tfRelativeCurrentPose * tfBasePose; - virtual void generateTrajectory() override; + tfGlobalPose.setRotation(tfGlobalPose.getRotation() * localquat); - virtual void createMarkers() override; + geometry_msgs::msg::PoseStamped globalPose; + tf2::toMsg(tfGlobalPose, globalPose.pose); + globalPose.header.frame_id = planePivotPose_.header.frame_id; + globalPose.header.stamp = rclcpp::Time(planePivotPose_.header.stamp) + + rclcpp::Duration::from_seconds(i * secondsPerSample); + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "]" << rclcpp::Time(globalPose.header.stamp).nanoseconds()); + + this->endEffectorTrajectory_.push_back(globalPose); + currentAngle += angleStep; + } + } + + virtual void createMarkers() override + { + CbMoveEndEffectorTrajectory::createMarkers(); + + tf2::Transform localdirection; + localdirection.setIdentity(); + localdirection.setOrigin(tf2::Vector3(0.12, 0, 0)); + auto frameid = planePivotPose_.header.frame_id; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frameid; + marker.header.stamp = getNode()->now(); + marker.ns = "trajectory"; + marker.id = beahiorMarkers_.markers.size(); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.01; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0; + marker.color.b = 1.0; + + geometry_msgs::msg::Point start, end; + start.x = planePivotPose_.pose.position.x; + start.y = planePivotPose_.pose.position.y; + start.z = planePivotPose_.pose.position.z; + + tf2::Transform basetransform; + tf2::fromMsg(planePivotPose_.pose, basetransform); + tf2::Transform endarrow = localdirection * basetransform; + + end.x = endarrow.getOrigin().x(); + end.y = endarrow.getOrigin().y(); + end.z = endarrow.getOrigin().z(); + + marker.pose.orientation.w = 1; + marker.points.push_back(start); + marker.points.push_back(end); + + beahiorMarkers_.markers.push_back(marker); + } protected: // the rotation axis (z-axis) and the center of rotation in the space @@ -57,7 +224,85 @@ class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory double deltaRadians_; private: - void computeCurrentEndEffectorPoseRelativeToPivot(); + void computeCurrentEndEffectorPoseRelativeToPivot() + { + // Use CpTfListener component for transform lookups + CpTfListener * tfListener = nullptr; + this->requiresComponent(tfListener, false); // Optional component + + tf2::Stamped endEffectorInPivotFrame; + + try + { + if (!tipLink_ || *tipLink_ == "") + { + tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); + } + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] waiting transform, pivot: '" + << planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'"); + + if (tfListener != nullptr) + { + // Use component-based TF listener (preferred) + auto transformOpt = + tfListener->lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time()); + + if (transformOpt) + { + tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame); + } + else + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_ + << " to " << planePivotPose_.header.frame_id); + return; + } + } + else + { + // Fallback to legacy TF2 usage if component not available + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() + << "] CpTfListener component not available, using legacy TF2 (consider " + "adding CpTfListener component)"); + tf2_ros::Buffer tfBuffer(getNode()->get_clock()); + tf2_ros::TransformListener tfListenerLegacy(tfBuffer); + + tf2::fromMsg( + tfBuffer.lookupTransform( + planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)), + endEffectorInPivotFrame); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM( + getLogger(), + "[" << getName() + << "] Exception in computeCurrentEndEffectorPoseRelativeToPivot: " << e.what()); + return; + } + + // tf2::Transform endEffectorInBaseLinkFrame; + // tf2::fromMsg(currentRobotEndEffectorPose.pose, endEffectorInBaseLinkFrame); + + // tf2::Transform endEffectorInPivotFrame = globalBaseLink * endEffectorInBaseLinkFrame; // pose composition + + // now pivot and EndEffector share a common reference frame (let say map) + // now get the current pose from the pivot reference frame with inverse composition + tf2::Transform pivotTransform; + tf2::fromMsg(planePivotPose_.pose, pivotTransform); + tf2::Transform invertedNewReferenceFrame = pivotTransform.inverse(); + + tf2::Transform currentPoseRelativeToPivot = invertedNewReferenceFrame * endEffectorInPivotFrame; + + geometry_msgs::msg::Pose finalEndEffectorRelativePose; + tf2::toMsg(currentPoseRelativeToPivot, finalEndEffectorRelativePose); + relativeInitialPose_ = finalEndEffectorRelativePose; + } }; } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_detach_object.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_detach_object.hpp index 3b2dc0176..f7e3e8fb4 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_detach_object.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_detach_object.hpp @@ -26,12 +26,66 @@ namespace cl_moveit2z { -// releases the current attached object +/** + * @brief Client behavior that detaches the currently attached collision object + * + * This behavior detaches whatever object is currently attached to the robot's + * gripper (tracked in CpGraspingComponent::currentAttachedObjectName) and + * removes it from the planning scene. Posts success or failure event based + * on the detach operation result. + */ class CbDetachObject : public smacc2::SmaccAsyncClientBehavior { public: - virtual void onEntry() override; + /** + * @brief Called when the behavior is entered + * + * Retrieves the currently attached object name from CpGraspingComponent, + * detaches it from the gripper, and removes it from the planning scene. + */ + inline void onEntry() override + { + cl_moveit2z::CpGraspingComponent * graspingComponent; + this->requiresComponent(graspingComponent); + + cl_moveit2z::ClMoveit2z * moveGroupClient; + this->requiresClient(moveGroupClient); + + if (graspingComponent->currentAttachedObjectName) + { + RCLCPP_INFO_STREAM( + getLogger(), + "[CbDetachObject] Detaching object: " << *(graspingComponent->currentAttachedObjectName)); + + auto & planningSceneInterface = moveGroupClient->planningSceneInterface; + auto res = moveGroupClient->moveGroupClientInterface->detachObject( + *(graspingComponent->currentAttachedObjectName)); - virtual void onExit() override; + planningSceneInterface->removeCollisionObjects( + {*(graspingComponent->currentAttachedObjectName)}); + + if (res) + { + RCLCPP_INFO(getLogger(), "[CbDetachObject] Detach succeeded"); + this->postSuccessEvent(); + } + else + { + RCLCPP_ERROR(getLogger(), "[CbDetachObject] Detach failed"); + this->postFailureEvent(); + } + } + else + { + RCLCPP_ERROR(getLogger(), "[CbDetachObject] No object currently attached"); + this->postFailureEvent(); + } + } + + /** + * @brief Called when the behavior is exited + */ + inline void onExit() override {} }; + } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp index af8c0dfcc..1fa44e470 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp @@ -18,19 +18,117 @@ * *****************************************************************************************************************/ +#include +#include +#include +#include +#include +#include #include "cb_circular_pivot_motion.hpp" +using namespace std::chrono_literals; + namespace cl_moveit2z { // spins the end effector joint (or any other arbitrary joint etting the tipLink parameter) class CbEndEffectorRotate : public CbCircularPivotMotion { public: - CbEndEffectorRotate(double deltaRadians, std::optional tipLink = std::nullopt); + CbEndEffectorRotate(double deltaRadians, std::optional tipLink = std::nullopt) + : CbCircularPivotMotion(tipLink) + { + deltaRadians_ = deltaRadians; + } + + virtual ~CbEndEffectorRotate() {} + + virtual void onEntry() override + { + // Use CpTfListener component for transform lookups + CpTfListener * tfListener = nullptr; + this->requiresComponent(tfListener, false); // Optional component + + tf2::Stamped endEffectorInPivotFrame; + + int attempts = 3; + + this->requiresClient(movegroupClient_); + if (!tipLink_) + { + tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); + RCLCPP_WARN_STREAM( + getLogger(), + "[" << getName() << "] tip unspecified, using default end effector: " << *tipLink_); + } + + while (attempts > 0) + { + try + { + auto pivotFrameName = + this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); + + if (tfListener != nullptr) + { + // Use component-based TF listener (preferred) + auto transformOpt = + tfListener->lookupTransform(pivotFrameName, *tipLink_, rclcpp::Time()); + + if (transformOpt) + { + tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame); + tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose); + this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_; + this->planePivotPose_.header.stamp = + rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count()); + break; + } + else + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_ + << " to " << pivotFrameName << ". Attempt countdown: " << attempts); + rclcpp::sleep_for(500ms); + attempts--; + } + } + else + { + // Fallback to legacy TF2 usage if component not available + RCLCPP_WARN_STREAM( + getLogger(), + "[" << getName() + << "] CpTfListener component not available, using legacy TF2 (consider " + "adding CpTfListener component)"); + tf2_ros::Buffer tfBuffer(getNode()->get_clock()); + tf2_ros::TransformListener tfListenerLegacy(tfBuffer); + + tf2::fromMsg( + tfBuffer.lookupTransform( + pivotFrameName, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)), + endEffectorInPivotFrame); + + tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose); + this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_; + this->planePivotPose_.header.stamp = + rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count()); + break; + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM(getLogger(), e.what() << ". Attempt countdown: " << attempts); + rclcpp::sleep_for(500ms); + attempts--; + } + } - virtual ~CbEndEffectorRotate(); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] pivotPose: " << planePivotPose_); - virtual void onEntry() override; + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] calling base CbCircularPivotMotion::onEntry"); + CbCircularPivotMotion::onEntry(); + } std::optional tipLink; }; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.hpp deleted file mode 100644 index 44b7b4720..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ -#pragma once - -#include -#include - -namespace cl_moveit2z -{ -class CbExecuteLastTrajectory : public CbMoveEndEffectorTrajectory -{ -public: - CbExecuteLastTrajectory(); - - virtual ~CbExecuteLastTrajectory(); - - virtual void generateTrajectory(); - - virtual void onEntry() override; -}; - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.hpp deleted file mode 100644 index 220fd61e2..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -namespace cl_moveit2z -{ -// Performs a relative motion from the current end effector pose -// keeping the same orientation and just moving an offset in the position -class CbMoveCartesianRelative : public smacc2::SmaccAsyncClientBehavior -{ -public: - geometry_msgs::msg::Vector3 offset_; - - std::optional scalingFactor_; - - std::optional group_; - - CbMoveCartesianRelative(); - - CbMoveCartesianRelative(geometry_msgs::msg::Vector3 offset); - - virtual void onEntry() override; - - virtual void onExit() override; - - void moveRelativeCartesian( - moveit::planning_interface::MoveGroupInterface * movegroupClient, - geometry_msgs::msg::Vector3 & offset); - -public: - ClMoveit2z * moveGroupSmaccClient_; -}; -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.hpp index f8cc2fe97..7a54cc06e 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.hpp @@ -22,10 +22,13 @@ #include #include +#include #include #include #include "cb_move_end_effector_trajectory.hpp" +using namespace std::chrono_literals; + namespace cl_moveit2z { class CbMoveCartesianRelative2 : public CbMoveEndEffectorTrajectory @@ -35,14 +38,85 @@ class CbMoveCartesianRelative2 : public CbMoveEndEffectorTrajectory std::optional linearSpeed_m_s_; - CbMoveCartesianRelative2(std::string referenceFrame, std::string tipLink); + CbMoveCartesianRelative2(std::string referenceFrame, std::string tipLink) + : CbMoveEndEffectorTrajectory(tipLink) + { + globalFrame_ = referenceFrame; + } CbMoveCartesianRelative2( - std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset); + std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset) + : CbMoveEndEffectorTrajectory(tipLink) + { + globalFrame_ = referenceFrame; + offset_ = offset; + } + + virtual ~CbMoveCartesianRelative2() {} + + void generateTrajectory() override + { + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] generating trajectory"); + // at least 1 sample per centimeter (average) + const double METERS_PER_SAMPLE = 0.001; + + float dist_meters = 0; + tf2::Vector3 voffset; + tf2::fromMsg(offset_, voffset); + + float totallineardist = voffset.length(); + + int totalSamplesCount = totallineardist / METERS_PER_SAMPLE; + int steps = totallineardist / METERS_PER_SAMPLE; + + float interpolation_factor_step = 1.0 / totalSamplesCount; + + double secondsPerSample; + + if (!linearSpeed_m_s_) + { + linearSpeed_m_s_ = 0.1; + } + + secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_); + + tf2::Stamped currentEndEffectorTransform; + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current end effector pose"); + this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform); + + tf2::Transform finalEndEffectorTransform = currentEndEffectorTransform; + finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset); + + float linc = totallineardist / steps; // METERS_PER_SAMPLE with sign + float interpolation_factor = 0; + + rclcpp::Time startTime = getNode()->now(); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps); + + for (float i = 0; i < steps; i++) + { + interpolation_factor += interpolation_factor_step; + dist_meters += linc; + + tf2::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp( + finalEndEffectorTransform.getOrigin(), interpolation_factor); + + tf2::Transform pose; + pose.setOrigin(vi); + pose.setRotation(currentEndEffectorTransform.getRotation()); + + geometry_msgs::msg::PoseStamped pointerPose; + tf2::toMsg(pose, pointerPose.pose); + pointerPose.header.frame_id = globalFrame_; + pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample); - virtual ~CbMoveCartesianRelative2(); + this->endEffectorTrajectory_.push_back(pointerPose); + } - void generateTrajectory() override; + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] Target End efector Pose: " << this->endEffectorTrajectory_.back()); + } private: std::string globalFrame_; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector.hpp index 34ddf59ee..6e24db970 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector.hpp @@ -20,9 +20,17 @@ #pragma once +#include #include +#include +#include +#include #include #include +#include + +using namespace std::chrono_literals; + namespace cl_moveit2z { class CbMoveEndEffector : public smacc2::SmaccAsyncClientBehavior @@ -32,15 +40,178 @@ class CbMoveEndEffector : public smacc2::SmaccAsyncClientBehavior std::string tip_link_; std::optional group_; - CbMoveEndEffector(); - CbMoveEndEffector(geometry_msgs::msg::PoseStamped target_pose, std::string tip_link = ""); + CbMoveEndEffector() {} + + CbMoveEndEffector(geometry_msgs::msg::PoseStamped target_pose, std::string tip_link = "") + : targetPose(target_pose) + { + tip_link_ = tip_link; + } + + virtual void onEntry() override + { + this->requiresClient(movegroupClient_); - virtual void onEntry() override; + if (this->group_) + { + RCLCPP_DEBUG( + getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector"); + moveit::planning_interface::MoveGroupInterface move_group(getNode(), *group_); + this->moveToAbsolutePose(move_group, targetPose); + RCLCPP_DEBUG( + getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed"); + } + else + { + RCLCPP_DEBUG( + getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector"); + this->moveToAbsolutePose(*(movegroupClient_->moveGroupClientInterface), targetPose); + RCLCPP_DEBUG( + getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed"); + } + } protected: bool moveToAbsolutePose( moveit::planning_interface::MoveGroupInterface & moveGroupInterface, - geometry_msgs::msg::PoseStamped & targetObjectPose); + geometry_msgs::msg::PoseStamped & targetObjectPose) + { + RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds"); + rclcpp::sleep_for(500ms); + + // Try to use CpMotionPlanner component (preferred) + CpMotionPlanner * motionPlanner = nullptr; + this->requiresComponent(motionPlanner, false); // Optional component + + bool success = false; + moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan; + + RCLCPP_INFO_STREAM( + getLogger(), "[CbMoveEndEffector] Target End effector Pose: " << targetObjectPose); + + if (motionPlanner != nullptr) + { + // Use component-based motion planner (preferred) + RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] Using CpMotionPlanner component for planning"); + + PlanningOptions options; + options.planningTime = 1.0; + options.poseReferenceFrame = targetObjectPose.header.frame_id; + + std::optional tipLinkOpt; + if (!tip_link_.empty()) + { + tipLinkOpt = tip_link_; + } + + auto result = motionPlanner->planToPose(targetObjectPose, tipLinkOpt, options); + + success = result.success; + if (success) + { + computedMotionPlan = result.plan; + RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] Planning succeeded (via CpMotionPlanner)"); + } + else + { + RCLCPP_WARN( + getLogger(), "[CbMoveEndEffector] Planning failed (via CpMotionPlanner): %s", + result.errorMessage.c_str()); + } + } + else + { + // Fallback to legacy direct API calls + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffector] CpMotionPlanner component not available, using legacy planning " + "(consider adding CpMotionPlanner component)"); + + moveGroupInterface.setPlanningTime(1.0); + moveGroupInterface.setPoseTarget(targetObjectPose, tip_link_); + moveGroupInterface.setPoseReferenceFrame(targetObjectPose.header.frame_id); + + success = + (moveGroupInterface.plan(computedMotionPlan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO( + getLogger(), "[CbMoveEndEffector] Planning %s (legacy mode)", + success ? "succeeded" : "FAILED"); + } + + // Execution + if (success) + { + // Try to use CpTrajectoryExecutor component (preferred) + CpTrajectoryExecutor * trajectoryExecutor = nullptr; + this->requiresComponent(trajectoryExecutor, false); // Optional component + + bool executionSuccess = false; + + if (trajectoryExecutor != nullptr) + { + // Use component-based trajectory executor (preferred) + RCLCPP_INFO( + getLogger(), "[CbMoveEndEffector] Using CpTrajectoryExecutor component for execution"); + + ExecutionOptions execOptions; + execOptions.trajectoryName = this->getName(); + + auto execResult = trajectoryExecutor->executePlan(computedMotionPlan, execOptions); + executionSuccess = execResult.success; + + if (executionSuccess) + { + RCLCPP_INFO( + getLogger(), "[CbMoveEndEffector] Execution succeeded (via CpTrajectoryExecutor)"); + } + else + { + RCLCPP_WARN( + getLogger(), "[CbMoveEndEffector] Execution failed (via CpTrajectoryExecutor): %s", + execResult.errorMessage.c_str()); + } + } + else + { + // Fallback to legacy direct execution + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffector] CpTrajectoryExecutor component not available, using legacy " + "execution " + "(consider adding CpTrajectoryExecutor component)"); + + auto executionResult = moveGroupInterface.execute(computedMotionPlan); + executionSuccess = (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + + RCLCPP_INFO( + getLogger(), "[CbMoveEndEffector] Execution %s (legacy mode)", + executionSuccess ? "succeeded" : "failed"); + } + + // Post events + if (executionSuccess) + { + movegroupClient_->postEventMotionExecutionSucceded(); + this->postSuccessEvent(); + } + else + { + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + } + } + else + { + RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] planning failed, skipping execution"); + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + } + + RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds"); + rclcpp::sleep_for(500ms); + + return success; + } ClMoveit2z * movegroupClient_; }; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.hpp deleted file mode 100644 index 872cd8895..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -namespace cl_moveit2z -{ -class CbMoveEndEffectorRelative : public smacc2::SmaccAsyncClientBehavior -{ -public: - geometry_msgs::msg::Transform transform_; - - std::optional group_; - - CbMoveEndEffectorRelative(); - - CbMoveEndEffectorRelative(geometry_msgs::msg::Transform transform); - - virtual void onEntry() override; - - virtual void onExit() override; - -protected: - void moveRelative( - moveit::planning_interface::MoveGroupInterface & moveGroupinterface, - geometry_msgs::msg::Transform & transformOffset); - - ClMoveit2z * movegroupClient_; -}; - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.hpp index b42f56e5e..29f10b071 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.hpp @@ -21,11 +21,20 @@ #pragma once #include +#include #include +#include +#include +#include +#include +#include +#include #include #include #include +using namespace std::chrono_literals; + namespace cl_moveit2z { template @@ -48,8 +57,7 @@ enum class ComputeJointTrajectoryErrorCode }; // this is a base behavior to define any kind of parameterized family of trajectories or motions -class CbMoveEndEffectorTrajectory : public smacc2::SmaccAsyncClientBehavior, - public smacc2::ISmaccUpdatable +class CbMoveEndEffectorTrajectory : public smacc2::SmaccAsyncClientBehavior { public: // std::string tip_link_; @@ -59,11 +67,16 @@ class CbMoveEndEffectorTrajectory : public smacc2::SmaccAsyncClientBehavior, std::optional allowInitialTrajectoryStateJointDiscontinuity_; - CbMoveEndEffectorTrajectory(std::optional tipLink = std::nullopt); + CbMoveEndEffectorTrajectory(std::optional tipLink = std::nullopt) : tipLink_(tipLink) + { + } CbMoveEndEffectorTrajectory( const std::vector & endEffectorTrajectory, - std::optional tipLink = std::nullopt); + std::optional tipLink = std::nullopt) + : tipLink_(tipLink), endEffectorTrajectory_(endEffectorTrajectory) + { + } template void onOrthogonalAllocation() @@ -94,22 +107,497 @@ class CbMoveEndEffectorTrajectory : public smacc2::SmaccAsyncClientBehavior, }; } - virtual void onEntry() override; + virtual void onEntry() override + { + this->requiresClient(movegroupClient_); + + // Get optional components for visualization + CpTrajectoryVisualizer * trajectoryVisualizer = nullptr; + this->requiresComponent(trajectoryVisualizer, false); // Optional component + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Generating end effector trajectory"); + + this->generateTrajectory(); + + if (this->endEffectorTrajectory_.size() == 0) + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << smacc2::demangleSymbol(typeid(*this).name()) + << "] No points in the trajectory. Skipping behavior."); + return; + } + + // Use CpTrajectoryVisualizer if available, otherwise use legacy marker system + if (trajectoryVisualizer != nullptr) + { + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] Setting trajectory visualization using CpTrajectoryVisualizer."); + trajectoryVisualizer->setTrajectory(this->endEffectorTrajectory_, "trajectory"); + } + else + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() + << "] Creating markers (legacy mode - consider adding " + "CpTrajectoryVisualizer component)."); + this->createMarkers(); + } + + moveit_msgs::msg::RobotTrajectory computedTrajectory; + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Computing joint space trajectory."); + + auto errorcode = computeJointSpaceTrajectory(computedTrajectory); - virtual void onExit() override; + bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS; - virtual void update() override; + CpTrajectoryHistory * trajectoryHistory; + this->requiresComponent(trajectoryHistory); + + if (!trajectoryGenerationSuccess) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << this->getName() << "] Incorrect trajectory. Posting failure event."); + if (trajectoryHistory != nullptr) + { + moveit_msgs::msg::MoveItErrorCodes error; + error.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error); + } + + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + + if (errorcode == ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY) + { + this->postJointDiscontinuityEvent(computedTrajectory); + } + else if (errorcode == ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE) + { + this->postIncorrectInitialStateEvent(computedTrajectory); + } + return; + } + else + { + if (trajectoryHistory != nullptr) + { + moveit_msgs::msg::MoveItErrorCodes error; + error.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error); + } + + this->executeJointSpaceTrajectory(computedTrajectory); + } + + // handle finishing events + } + + virtual void onExit() override + { + // Get optional components for visualization cleanup + CpTrajectoryVisualizer * trajectoryVisualizer = nullptr; + this->requiresComponent(trajectoryVisualizer, false); // Optional component + + if (trajectoryVisualizer != nullptr && autocleanmarkers) + { + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] Clearing trajectory markers via CpTrajectoryVisualizer."); + trajectoryVisualizer->clearMarkers(); + } + else if (autocleanmarkers) + { + // Legacy marker cleanup + std::lock_guard guard(m_mutex_); + for (auto & marker : this->beahiorMarkers_.markers) + { + marker.header.stamp = getNode()->now(); + marker.action = visualization_msgs::msg::Marker::DELETE; + } + + if (markersPub_) + { + markersPub_->publish(beahiorMarkers_); + } + } + } protected: ComputeJointTrajectoryErrorCode computeJointSpaceTrajectory( - moveit_msgs::msg::RobotTrajectory & computedJointTrajectory); + moveit_msgs::msg::RobotTrajectory & computedJointTrajectory) + { + // Try to use CpJointSpaceTrajectoryPlanner component (preferred) + CpJointSpaceTrajectoryPlanner * trajectoryPlanner = nullptr; + this->requiresComponent(trajectoryPlanner, false); // Optional component + + if (trajectoryPlanner != nullptr) + { + // Use component-based trajectory planner (preferred) + RCLCPP_INFO( + getLogger(), + "[CbMoveEndEffectorTrajectory] Using CpJointSpaceTrajectoryPlanner component for IK " + "trajectory generation"); + + JointTrajectoryOptions options; + if (tipLink_ && !tipLink_->empty()) + { + options.tipLink = *tipLink_; + } + if (allowInitialTrajectoryStateJointDiscontinuity_) + { + options.allowInitialDiscontinuity = *allowInitialTrajectoryStateJointDiscontinuity_; + } + + auto result = trajectoryPlanner->planFromWaypoints(endEffectorTrajectory_, options); + + if (result.success) + { + computedJointTrajectory = result.trajectory; + RCLCPP_INFO( + getLogger(), + "[CbMoveEndEffectorTrajectory] IK trajectory generation succeeded (via " + "CpJointSpaceTrajectoryPlanner)"); + return ComputeJointTrajectoryErrorCode::SUCCESS; + } + else + { + computedJointTrajectory = result.trajectory; // Still return partial trajectory + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffectorTrajectory] IK trajectory generation failed (via " + "CpJointSpaceTrajectoryPlanner): %s", + result.errorMessage.c_str()); + + // Map error codes + switch (result.errorCode) + { + case JointTrajectoryErrorCode::INCORRECT_INITIAL_STATE: + return ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE; + case JointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY: + return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY; + default: + return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY; + } + } + } + else + { + // Fallback to legacy IK trajectory generation + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffectorTrajectory] CpJointSpaceTrajectoryPlanner component not available, " + "using legacy IK trajectory generation (consider adding CpJointSpaceTrajectoryPlanner " + "component)"); + + // LEGACY IMPLEMENTATION (keep for backward compatibility) + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current state.. waiting"); + + auto currentState = movegroupClient_->moveGroupClientInterface->getCurrentState(100); + auto groupname = movegroupClient_->moveGroupClientInterface->getName(); + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting joint names"); + auto currentjointnames = + currentState->getJointModelGroup(groupname)->getActiveJointModelNames(); + + if (!tipLink_ || *tipLink_ == "") + { + tipLink_ = movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); + } + + std::vector jointPositions; + currentState->copyJointGroupPositions(groupname, jointPositions); + + std::vector> trajectory; + std::vector trajectoryTimeStamps; + + trajectory.push_back(jointPositions); + trajectoryTimeStamps.push_back(rclcpp::Duration(0s)); + + auto & first = endEffectorTrajectory_.front(); + rclcpp::Time referenceTime(first.header.stamp); + + std::vector discontinuityIndexes; + + int ikAttempts = 4; + for (size_t k = 0; k < this->endEffectorTrajectory_.size(); k++) + { + auto & pose = this->endEffectorTrajectory_[k]; + auto req = std::make_shared(); + + req->ik_request.ik_link_name = *tipLink_; + req->ik_request.robot_state.joint_state.name = currentjointnames; + req->ik_request.robot_state.joint_state.position = jointPositions; + req->ik_request.group_name = groupname; + req->ik_request.avoid_collisions = true; + req->ik_request.pose_stamped = pose; + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] IK request: " << k << " " << *req); + + auto resfut = iksrv_->async_send_request(req); + auto status = resfut.wait_for(3s); + + if (status == std::future_status::ready) + { + auto & prevtrajpoint = trajectory.back(); + auto res = resfut.get(); + + std::stringstream ss; + for (size_t j = 0; j < res->solution.joint_state.position.size(); j++) + { + auto & jointname = res->solution.joint_state.name[j]; + auto it = std::find(currentjointnames.begin(), currentjointnames.end(), jointname); + if (it != currentjointnames.end()) + { + int index = std::distance(currentjointnames.begin(), it); + jointPositions[index] = res->solution.joint_state.position[j]; + ss << jointname << "(" << index << "): " << jointPositions[index] << std::endl; + } + } + + // Continuity check + size_t jointindex = 0; + int discontinuityJointIndex = -1; + double discontinuityDeltaJointIndex = -1; + double deltajoint; + + bool check = k > 0 || !allowInitialTrajectoryStateJointDiscontinuity_ || + (allowInitialTrajectoryStateJointDiscontinuity_ && + !(*allowInitialTrajectoryStateJointDiscontinuity_)); + if (check) + { + for (jointindex = 0; jointindex < jointPositions.size(); jointindex++) + { + deltajoint = jointPositions[jointindex] - prevtrajpoint[jointindex]; + if (fabs(deltajoint) > 0.3) + { + discontinuityDeltaJointIndex = deltajoint; + discontinuityJointIndex = jointindex; + } + } + } + + if (ikAttempts > 0 && discontinuityJointIndex != -1) + { + k--; + ikAttempts--; + continue; + } + else + { + bool discontinuity = false; + if (ikAttempts == 0) + { + discontinuityIndexes.push_back(k); + discontinuity = true; + } + + ikAttempts = 4; + + if (discontinuity && discontinuityJointIndex != -1) + { + std::stringstream ss; + ss << "Traj[" << k << "/" << endEffectorTrajectory_.size() << "] " + << currentjointnames[discontinuityJointIndex] + << " IK discontinuity : " << discontinuityDeltaJointIndex << std::endl + << "prev joint value: " << prevtrajpoint[discontinuityJointIndex] << std::endl + << "current joint value: " << jointPositions[discontinuityJointIndex] << std::endl; + + ss << std::endl; + for (size_t ji = 0; ji < jointPositions.size(); ji++) + { + ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl; + } + + for (size_t kindex = 0; kindex < trajectory.size(); kindex++) + { + ss << "[" << kindex << "]: " << trajectory[kindex][discontinuityJointIndex] + << std::endl; + } + + if (k == 0) + { + ss + << "This is the first posture of the trajectory. Maybe the robot initial posture " + "is " + "not coincident to the initial posture of the generated joint trajectory." + << std::endl; + } + + RCLCPP_ERROR_STREAM(getLogger(), ss.str()); + + trajectory.push_back(jointPositions); + rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime; + trajectoryTimeStamps.push_back(durationFromStart); + continue; + } + else + { + trajectory.push_back(jointPositions); + rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime; + trajectoryTimeStamps.push_back(durationFromStart); + + RCLCPP_DEBUG_STREAM(getLogger(), "IK solution: " << res->solution.joint_state); + RCLCPP_DEBUG_STREAM(getLogger(), "trajpoint: " << std::endl << ss.str()); + } + } + } + else + { + RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] wrong IK call"); + } + } + + computedJointTrajectory.joint_trajectory.joint_names = currentjointnames; + int i = 0; + for (auto & p : trajectory) + { + if (i == 0) // Skip current state + { + i++; + continue; + } + + trajectory_msgs::msg::JointTrajectoryPoint jp; + jp.positions = p; + jp.time_from_start = trajectoryTimeStamps[i]; + computedJointTrajectory.joint_trajectory.points.push_back(jp); + i++; + } + + if (discontinuityIndexes.size()) + { + if (discontinuityIndexes[0] == 0) + return ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE; + else + return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY; + } + + return ComputeJointTrajectoryErrorCode::SUCCESS; + } + } void executeJointSpaceTrajectory( - const moveit_msgs::msg::RobotTrajectory & computedJointTrajectory); + const moveit_msgs::msg::RobotTrajectory & computedJointTrajectory) + { + RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory"); + + // Try to use CpTrajectoryExecutor component (preferred) + CpTrajectoryExecutor * trajectoryExecutor = nullptr; + this->requiresComponent(trajectoryExecutor, false); // Optional component + + bool executionSuccess = false; + + if (trajectoryExecutor != nullptr) + { + // Use component-based trajectory executor (preferred) + RCLCPP_INFO( + getLogger(), + "[CbMoveEndEffectorTrajectory] Using CpTrajectoryExecutor component for execution"); + + ExecutionOptions execOptions; + execOptions.trajectoryName = this->getName(); + + auto execResult = trajectoryExecutor->execute(computedJointTrajectory, execOptions); + executionSuccess = execResult.success; + + if (executionSuccess) + { + RCLCPP_INFO( + getLogger(), + "[CbMoveEndEffectorTrajectory] Execution succeeded (via CpTrajectoryExecutor)"); + } + else + { + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffectorTrajectory] Execution failed (via CpTrajectoryExecutor): %s", + execResult.errorMessage.c_str()); + } + } + else + { + // Fallback to legacy direct execution + RCLCPP_WARN( + getLogger(), + "[CbMoveEndEffectorTrajectory] CpTrajectoryExecutor component not available, using legacy " + "execution (consider adding CpTrajectoryExecutor component)"); + + auto executionResult = + this->movegroupClient_->moveGroupClientInterface->execute(computedJointTrajectory); + executionSuccess = (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + + RCLCPP_INFO( + getLogger(), "[CbMoveEndEffectorTrajectory] Execution %s (legacy mode)", + executionSuccess ? "succeeded" : "failed"); + } + + // Post events + if (executionSuccess) + { + RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution succeeded"); + movegroupClient_->postEventMotionExecutionSucceded(); + this->postSuccessEvent(); + } + else + { + this->postMotionExecutionFailureEvents(); + this->postFailureEvent(); + } + } + + virtual void generateTrajectory() + { + // bypass current trajectory, overridden in derived classes + // this->endEffectorTrajectory_ = ... + } - virtual void generateTrajectory() = 0; + virtual void createMarkers() + { + tf2::Transform localdirection; + localdirection.setIdentity(); + localdirection.setOrigin(tf2::Vector3(0.05, 0, 0)); + auto frameid = this->endEffectorTrajectory_.front().header.frame_id; - virtual void createMarkers(); + for (auto & pose : this->endEffectorTrajectory_) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frameid; + marker.header.stamp = getNode()->now(); + marker.ns = "trajectory"; + marker.id = this->beahiorMarkers_.markers.size(); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.005; + marker.scale.y = 0.01; + marker.scale.z = 0.01; + marker.color.a = 0.8; + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + + geometry_msgs::msg::Point start, end; + start.x = 0; + start.y = 0; + start.z = 0; + + tf2::Transform basetransform; + tf2::fromMsg(pose.pose, basetransform); + // tf2::Transform endarrow = localdirection * basetransform; + + end.x = localdirection.getOrigin().x(); + end.y = localdirection.getOrigin().y(); + end.z = localdirection.getOrigin().z(); + + marker.pose.position = pose.pose.position; + marker.pose.orientation = pose.pose.orientation; + marker.points.push_back(start); + marker.points.push_back(end); + + beahiorMarkers_.markers.push_back(marker); + } + } std::vector endEffectorTrajectory_; @@ -118,14 +606,71 @@ class CbMoveEndEffectorTrajectory : public smacc2::SmaccAsyncClientBehavior, visualization_msgs::msg::MarkerArray beahiorMarkers_; void getCurrentEndEffectorPose( - std::string globalFrame, tf2::Stamped & currentEndEffectorTransform); + std::string globalFrame, tf2::Stamped & currentEndEffectorTransform) + { + // Use CpTfListener component for transform lookups + CpTfListener * tfListener = nullptr; + this->requiresComponent(tfListener, false); // Optional component + + try + { + if (!tipLink_ || *tipLink_ == "") + { + tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); + } + + if (tfListener != nullptr) + { + // Use component-based TF listener (preferred) + auto transformOpt = tfListener->lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0)); + if (transformOpt) + { + tf2::fromMsg(transformOpt.value(), currentEndEffectorTransform); + } + else + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_ + << " to " << globalFrame); + } + } + else + { + // Fallback to legacy TF2 usage if component not available + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() + << "] CpTfListener component not available, using legacy TF2 (consider " + "adding CpTfListener component)"); + tf2_ros::Buffer tfBuffer(getNode()->get_clock()); + tf2_ros::TransformListener tfListenerLegacy(tfBuffer); + + tf2::fromMsg( + tfBuffer.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), rclcpp::Duration(10s)), + currentEndEffectorTransform); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] Exception in getCurrentEndEffectorPose: " << e.what()); + } + } private: - void initializeROS(); + void initializeROS() + { + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] initializing ros"); - rclcpp::Publisher::SharedPtr markersPub_; + auto nh = this->getNode(); + + // Only create marker publisher for legacy mode (when CpTrajectoryVisualizer is not used) + markersPub_ = nh->create_publisher( + "trajectory_markers", rclcpp::QoS(1)); - std::atomic markersInitialized_; + iksrv_ = nh->create_client("/compute_ik"); + } + + rclcpp::Publisher::SharedPtr markersPub_; rclcpp::Client::SharedPtr iksrv_; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints.hpp index c6573c4c8..d6fa9457d 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints.hpp @@ -20,10 +20,15 @@ #pragma once +#include +#include #include +#include #include #include +#include +#include #include namespace cl_moveit2z @@ -35,13 +40,200 @@ class CbMoveJoints : public smacc2::SmaccAsyncClientBehavior std::map jointValueTarget_; std::optional group_; - CbMoveJoints(); - CbMoveJoints(const std::map & jointValueTarget); - virtual void onEntry() override; - virtual void onExit() override; + CbMoveJoints() {} + + CbMoveJoints(const std::map & jointValueTarget) + : jointValueTarget_(jointValueTarget) + { + } + + virtual void onEntry() override + { + this->requiresClient(movegroupClient_); + + if (this->group_) + { + moveit::planning_interface::MoveGroupInterface move_group( + getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_))); + this->moveJoints(move_group); + } + else + { + this->moveJoints(*movegroupClient_->moveGroupClientInterface); + } + } + + virtual void onExit() override {} protected: - void moveJoints(moveit::planning_interface::MoveGroupInterface & moveGroupInterface); + static std::string currentJointStatesToString( + moveit::planning_interface::MoveGroupInterface & moveGroupInterface, + std::map & targetJoints) + { + auto state = moveGroupInterface.getCurrentState(); + + if (state == nullptr) return std::string(); + + auto vnames = state->getVariableNames(); + + std::stringstream ss; + + for (auto & tgj : targetJoints) + { + auto it = std::find(vnames.begin(), vnames.end(), tgj.first); + auto index = std::distance(vnames.begin(), it); + + ss << tgj.first << ":" << state->getVariablePosition(index) << std::endl; + } + + return ss.str(); + } + + void moveJoints(moveit::planning_interface::MoveGroupInterface & moveGroupInterface) + { + if (jointValueTarget_.size() == 0) + { + RCLCPP_WARN(getLogger(), "[CbMoveJoints] No joint value specified. Skipping planning call."); + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + return; + } + + // Try to use CpMotionPlanner component (preferred) + CpMotionPlanner * motionPlanner = nullptr; + this->requiresComponent(motionPlanner, false); // Optional component + + bool success = false; + moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan; + + if (motionPlanner != nullptr) + { + // Use component-based motion planner (preferred) + RCLCPP_INFO(getLogger(), "[CbMoveJoints] Using CpMotionPlanner component for joint planning"); + + PlanningOptions options; + if (scalingFactor_) + { + options.maxVelocityScaling = *scalingFactor_; + } + + auto result = motionPlanner->planToJointTarget(jointValueTarget_, options); + + success = result.success; + if (success) + { + computedMotionPlan = result.plan; + RCLCPP_INFO(getLogger(), "[CbMoveJoints] Planning succeeded (via CpMotionPlanner)"); + } + else + { + RCLCPP_WARN( + getLogger(), "[CbMoveJoints] Planning failed (via CpMotionPlanner): %s", + result.errorMessage.c_str()); + } + } + else + { + // Fallback to legacy direct API calls + RCLCPP_WARN( + getLogger(), + "[CbMoveJoints] CpMotionPlanner component not available, using legacy planning " + "(consider adding CpMotionPlanner component)"); + + if (scalingFactor_) moveGroupInterface.setMaxVelocityScalingFactor(*scalingFactor_); + + moveGroupInterface.setJointValueTarget(jointValueTarget_); + + auto result = moveGroupInterface.plan(computedMotionPlan); + + success = (result == moveit::core::MoveItErrorCode::SUCCESS); + + RCLCPP_INFO( + getLogger(), "[CbMoveJoints] Planning %s (legacy mode, code: %d)", + success ? "SUCCESS" : "FAILED", result.val); + } + + // Execution + if (success) + { + // Try to use CpTrajectoryExecutor component (preferred) + CpTrajectoryExecutor * trajectoryExecutor = nullptr; + this->requiresComponent(trajectoryExecutor, false); // Optional component + + bool executionSuccess = false; + + if (trajectoryExecutor != nullptr) + { + // Use component-based trajectory executor (preferred) + RCLCPP_INFO( + getLogger(), "[CbMoveJoints] Using CpTrajectoryExecutor component for execution"); + + ExecutionOptions execOptions; + execOptions.trajectoryName = this->getName(); + if (scalingFactor_) + { + execOptions.maxVelocityScaling = *scalingFactor_; + } + + auto execResult = trajectoryExecutor->executePlan(computedMotionPlan, execOptions); + executionSuccess = execResult.success; + + if (executionSuccess) + { + RCLCPP_INFO(getLogger(), "[CbMoveJoints] Execution succeeded (via CpTrajectoryExecutor)"); + } + else + { + RCLCPP_WARN( + getLogger(), "[CbMoveJoints] Execution failed (via CpTrajectoryExecutor): %s", + execResult.errorMessage.c_str()); + } + } + else + { + // Fallback to legacy direct execution + RCLCPP_WARN( + getLogger(), + "[CbMoveJoints] CpTrajectoryExecutor component not available, using legacy execution " + "(consider adding CpTrajectoryExecutor component)"); + + auto executionResult = moveGroupInterface.execute(computedMotionPlan); + executionSuccess = (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + + RCLCPP_INFO( + getLogger(), "[CbMoveJoints] Execution %s (legacy mode)", + executionSuccess ? "succeeded" : "failed"); + } + + // Post events + if (executionSuccess) + { + RCLCPP_INFO_STREAM( + getLogger(), + "[" << this->getName() << "] motion execution succeeded. Throwing success event."); + movegroupClient_->postEventMotionExecutionSucceded(); + this->postSuccessEvent(); + } + else + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."); + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + } + } + else + { + auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_); + RCLCPP_WARN_STREAM( + getLogger(), "[" << this->getName() << "] planning failed. Throwing fail event." + << std::endl + << statestr); + movegroupClient_->postEventMotionExecutionFailed(); + this->postFailureEvent(); + } + } + ClMoveit2z * movegroupClient_; }; } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints_relative.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints_relative.hpp deleted file mode 100644 index ff7488d29..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_joints_relative.hpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_known_state.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_known_state.hpp index 7a96aac7c..753d68ae6 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_known_state.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_known_state.hpp @@ -20,6 +20,10 @@ #pragma once +#include +#include +#include +#include #include #include #include "cb_move_joints.hpp" @@ -29,13 +33,110 @@ namespace cl_moveit2z class CbMoveKnownState : public CbMoveJoints { public: - CbMoveKnownState(std::string pkg, std::string config_path); - virtual ~CbMoveKnownState(); + CbMoveKnownState(std::string pkg, std::string config_path) : pkg_(pkg), config_path_(config_path) + { + } - void onEntry() override; + virtual ~CbMoveKnownState() {} + + void onEntry() override + { + jointValueTarget_ = loadJointStatesFromFile(pkg_, config_path_); + CbMoveJoints::onEntry(); + } private: - std::map loadJointStatesFromFile(std::string pkg, std::string filepath); +#define HAVE_NEW_YAMLCPP + std::map loadJointStatesFromFile(std::string pkg, std::string filepath) + { + //auto pkgpath = ros::package::getPath(pkg); + std::string pkgpath = ament_index_cpp::get_package_share_directory(pkg); + + std::map jointStates; + + if (pkgpath == "") + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] package not found for the known poses file: " << pkg + << std::endl + << " [IGNORING BEHAVIOR]"); + return jointStates; + } + + filepath = pkgpath + "/" + filepath; + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] Opening file with joint known state: " << filepath); + + if (std::filesystem::exists(filepath)) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] known state file exists: " << filepath); + } + else + { + RCLCPP_ERROR_STREAM( + getLogger(), "[" << getName() << "] known state file does not exists: " << filepath); + } + + std::ifstream ifs(filepath.c_str(), std::ifstream::in); + if (ifs.good() == false) + { + RCLCPP_ERROR_STREAM( + getLogger(), + "[" << getName() << "] Error opening file with joint known states: " << filepath); + throw std::string("joint state files not found"); + } + + try + { +#ifdef HAVE_NEW_YAMLCPP + YAML::Node node = YAML::Load(ifs); +#else + YAML::Parser parser(ifs); + parser.GetNextDocument(node); +#endif + +#ifdef HAVE_NEW_YAMLCPP + const YAML::Node & wp_node_tmp = node["joint_states"]; + const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL; +#else + const YAML::Node * wp_node = node.FindValue("waypoints"); +#endif + + if (wp_node != NULL) + { + try + { + for (YAML::const_iterator it = wp_node->begin(); it != wp_node->end(); ++it) + { + std::string key = it->first.as(); + double value = it->second.as(); + RCLCPP_DEBUG_STREAM(getLogger(), " joint - " << key << ": " << value); + jointStates[key] = value; + } + + return jointStates; + } + catch (std::exception & ex) + { + RCLCPP_ERROR(getLogger(), "trying to convert to map, failed, errormsg: %s", ex.what()); + } + + RCLCPP_INFO_STREAM(getLogger(), "Parsed " << jointStates.size() << " joint entries."); + } + else + { + RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any jointStates in the provided yaml file."); + } + } + catch (const YAML::ParserException & ex) + { + RCLCPP_ERROR_STREAM( + getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what()); + } + return jointStates; + } std::string pkg_; std::string config_path_; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.hpp deleted file mode 100644 index 967883c6b..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "cb_move_joints.hpp" - -namespace cl_moveit2z -{ -class CbMoveLastTrajectoryInitialState : public CbMoveJoints -{ -public: - CbMoveLastTrajectoryInitialState(); - - CbMoveLastTrajectoryInitialState(int backIndex); - - virtual ~CbMoveLastTrajectoryInitialState(); - - virtual void onEntry() override; - -private: - int backIndex_ = -1; -}; -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_named_target.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_named_target.hpp deleted file mode 100644 index 3fcaa3bfd..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_named_target.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -#include -#include - -namespace cl_moveit2z -{ -//named targets are configured in the urdf file -class CbMoveNamedTarget : public smacc2::SmaccAsyncClientBehavior -{ -protected: - ClMoveit2z * movegroupClient_; - std::string namedTarget_; - -public: - CbMoveNamedTarget(std::string namedtarget); - - virtual void onEntry() override; - - virtual void onExit() override; - - std::map getNamedTargetValues(); -}; -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_pouring_motion.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_pouring_motion.hpp deleted file mode 100644 index 9f9ac9511..000000000 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_pouring_motion.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ -#pragma once - -#include -#include "cb_move_end_effector_trajectory.hpp" - -namespace cl_moveit2z -{ -class CbCircularPouringMotion : public CbMoveEndEffectorTrajectory -{ -public: - std::optional angularSpeed_rad_s_; - - std::optional linearSpeed_m_s_; - - CbCircularPouringMotion( - const geometry_msgs::msg::Point & pivotPoint, double deltaHeight, std::string tipLink, - std::string globalFrame); - - virtual void generateTrajectory() override; - - virtual void createMarkers() override; - - geometry_msgs::msg::Vector3 directionVector_; - - // relative position of the "lid" of the bottle in the end effector reference frame. - // that point is the one that must do the linear motion - geometry_msgs::msg::Pose pointerRelativePose_; - -protected: - geometry_msgs::msg::Point relativePivotPoint_; - - // distance in meters from the initial pose to the bottom/top direction in z axe - double deltaHeight_; - - std::vector pointerTrajectory_; - -private: - void computeCurrentEndEffectorPoseRelativeToPivot(); - - std::string globalFrame_; -}; - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.hpp index d026a4991..b0d1365c3 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.hpp @@ -28,15 +28,45 @@ namespace cl_moveit2z class CbUndoLastTrajectory : public CbMoveEndEffectorTrajectory { public: - CbUndoLastTrajectory(); + CbUndoLastTrajectory() {} - CbUndoLastTrajectory(int backIndex); + CbUndoLastTrajectory(int backIndex) : backIndex_(backIndex) {} - virtual ~CbUndoLastTrajectory(); + virtual ~CbUndoLastTrajectory() {} - virtual void onEntry() override; + virtual void onEntry() override + { + CpTrajectoryHistory * trajectoryHistory; + this->requiresComponent(trajectoryHistory); + this->requiresClient(movegroupClient_); - virtual void generateTrajectory(); + if (trajectoryHistory->getLastTrajectory(backIndex_, trajectory)) + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() << "] reversing last trajectory [" << backIndex_ << "]"); + + auto initialTime = trajectory.joint_trajectory.points.back().time_from_start; + + reversed = trajectory; + + std::reverse( + reversed.joint_trajectory.points.begin(), reversed.joint_trajectory.points.end()); + + for (auto & jp : reversed.joint_trajectory.points) + { + jp.time_from_start = rclcpp::Duration(initialTime) - rclcpp::Duration(jp.time_from_start); + } + + this->executeJointSpaceTrajectory(reversed); + } + else + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() << "] could not undo last trajectory, trajectory not found."); + } + } + + virtual void generateTrajectory() {} private: int backIndex_ = -1; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/common.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/common.hpp index 418dcf188..172098a2b 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/common.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/common.hpp @@ -25,18 +25,50 @@ #include #include -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg); +// All stream operators are now inline for header-only implementation +// CRITICAL: Order matters! Primitive types first, then compound types that use them -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Transform & msg); +// Primitive geometry types (no dependencies) +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Vector3 & msg) +{ + return out << "[ " << msg.x << " " << msg.y << " " << msg.z << "]"; +} -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg); +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg) +{ + return out << "[ " << msg.x << " " << msg.y << " " << msg.z << "]"; +} -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg); +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg) +{ + return out << " Quaternion[" << msg.x << " , " << msg.y << " , " << msg.z << ", w:" << msg.w; +} -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg); +// Compound types (depend on primitive types above) +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Transform & msg) +{ + return out << "Translation[" << msg.translation << "], Rotation[" << msg.rotation << "]"; +} -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Vector3 & msg); +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg) +{ + return out << "Position[" << msg.position << "], Orientation[" << msg.orientation << "]"; +} -std::ostream & operator<<(std::ostream & out, const moveit_msgs::srv::GetPositionIK::Request & msg); +inline std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg) +{ + return out << "[serialization geometry_msgs::msg::PoseStamped] frame_id: " << msg.header.frame_id + << ", pose: " << msg.pose; +} -std::ostream & operator<<(std::ostream & out, const sensor_msgs::msg::JointState & msg); +inline std::ostream & operator<<( + std::ostream & out, const moveit_msgs::srv::GetPositionIK::Request & msg) +{ + return out << "[moveit_msgs::srv::GetPositionIK::Request] position[" + << msg.ik_request.pose_stamped << "]"; +} + +inline std::ostream & operator<<(std::ostream & out, const sensor_msgs::msg::JointState & /*msg*/) +{ + return out << "[sensor_msgs::msg::JointState]"; +} diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_grasping_objects.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_grasping_objects.hpp index 08080498b..9a51973df 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_grasping_objects.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_grasping_objects.hpp @@ -38,10 +38,46 @@ class CpGraspingComponent : public smacc2::ISmaccComponent std::optional currentAttachedObjectName; - bool getGraspingObject(std::string name, moveit_msgs::msg::CollisionObject & object); + inline bool getGraspingObject(std::string name, moveit_msgs::msg::CollisionObject & object) + { + if (this->graspingObjects.count(name)) + { + object = this->graspingObjects[name]; + return true; + } + else + { + return false; + } + } - void createGraspableBox( - std::string frameid, float x, float y, float z, float xl, float yl, float zl); + inline void createGraspableBox( + std::string frameid, float x, float y, float z, float xl, float yl, float zl) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] creating grasping object in planning scene: " << frameid); + moveit_msgs::msg::CollisionObject collision; + auto boxname = frameid; + ; + collision.id = boxname; + collision.header.frame_id = frameid; + + collision.primitives.resize(1); + collision.primitives[0].type = collision.primitives[0].BOX; + collision.primitives[0].dimensions.resize(3); + + collision.primitives[0].dimensions[0] = xl; + collision.primitives[0].dimensions[1] = yl; + collision.primitives[0].dimensions[2] = zl; + + collision.primitive_poses.resize(1); + collision.primitive_poses[0].position.x = x; + collision.primitive_poses[0].position.y = y; + collision.primitive_poses[0].position.z = z; + collision.primitive_poses[0].orientation.w = 1.0; + + graspingObjects[boxname] = collision; + } }; } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_joint_space_trajectory_planner.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_joint_space_trajectory_planner.hpp new file mode 100644 index 000000000..1eb17cbe4 --- /dev/null +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_joint_space_trajectory_planner.hpp @@ -0,0 +1,346 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace cl_moveit2z +{ +/** + * @brief Error codes for joint space trajectory computation + */ +enum class JointTrajectoryErrorCode +{ + SUCCESS, + INCORRECT_INITIAL_STATE, + JOINT_TRAJECTORY_DISCONTINUITY, + IK_SOLUTION_FAILED, + EMPTY_WAYPOINT_LIST +}; + +/** + * @brief Configuration options for joint space trajectory planning + */ +struct JointTrajectoryOptions +{ + std::optional tipLink; + std::optional groupName; + bool allowInitialDiscontinuity = false; + double maxJointDiscontinuity = 0.3; // radians (~17 degrees) + int ikAttempts = 4; + bool avoidCollisions = true; + std::chrono::seconds ikTimeout = 3s; +}; + +/** + * @brief Result of a joint space trajectory planning operation + */ +struct JointTrajectoryResult +{ + bool success; + JointTrajectoryErrorCode errorCode; + moveit_msgs::msg::RobotTrajectory trajectory; + std::string errorMessage; + std::vector discontinuityIndexes; + + JointTrajectoryResult() + : success(false), errorCode(JointTrajectoryErrorCode::IK_SOLUTION_FAILED), errorMessage("") + { + } +}; + +/** + * @brief Component for joint space trajectory generation from Cartesian waypoints + * + * This component centralizes IK-based trajectory generation: + * - Computes IK solutions for Cartesian waypoint sequences + * - Validates trajectory continuity + * - Detects and reports joint discontinuities + * - Handles initial state validation + * + * Pattern: Trajectory planner with IK service integration + */ +class CpJointSpaceTrajectoryPlanner : public smacc2::ISmaccComponent +{ +public: + CpJointSpaceTrajectoryPlanner() = default; + virtual ~CpJointSpaceTrajectoryPlanner() = default; + + /** + * @brief Initialize the component and create IK service client + */ + inline void onInitialize() override + { + this->requiresClient(moveit2zClient_); + + // Create IK service client + ikClient_ = getNode()->create_client("/compute_ik"); + + RCLCPP_INFO(getLogger(), "[CpJointSpaceTrajectoryPlanner] Component initialized"); + } + + /** + * @brief Compute joint space trajectory from Cartesian waypoints + * + * @param waypoints Vector of Cartesian poses to follow + * @param options Planning configuration options + * @return JointTrajectoryResult with success status and computed trajectory + */ + inline JointTrajectoryResult planFromWaypoints( + const std::vector & waypoints, + const JointTrajectoryOptions & options = {}) + { + JointTrajectoryResult result; + + if (waypoints.empty()) + { + result.errorCode = JointTrajectoryErrorCode::EMPTY_WAYPOINT_LIST; + result.errorMessage = "No waypoints provided"; + RCLCPP_WARN(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + return result; + } + + if (!moveit2zClient_) + { + result.errorMessage = "ClMoveit2z client not available"; + RCLCPP_ERROR(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + return result; + } + + try + { + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + // Get current robot state + RCLCPP_INFO( + getLogger(), + "[CpJointSpaceTrajectoryPlanner] Getting current state for trajectory planning"); + auto currentState = moveGroup->getCurrentState(100); + if (!currentState) + { + result.errorMessage = "Failed to get current robot state"; + RCLCPP_ERROR( + getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + return result; + } + + // Determine group name and tip link + std::string groupName = options.groupName.value_or(moveGroup->getName()); + std::string tipLink = options.tipLink.value_or(moveGroup->getEndEffectorLink()); + + RCLCPP_INFO( + getLogger(), "[CpJointSpaceTrajectoryPlanner] Planning for group '%s' with tip link '%s'", + groupName.c_str(), tipLink.c_str()); + + // Get joint names + auto jointNames = currentState->getJointModelGroup(groupName)->getActiveJointModelNames(); + + // Initialize trajectory with current state + std::vector jointPositions; + currentState->copyJointGroupPositions(groupName, jointPositions); + + std::vector> trajectory; + std::vector trajectoryTimeStamps; + + trajectory.push_back(jointPositions); + trajectoryTimeStamps.push_back(rclcpp::Duration(0s)); + + // Reference time for trajectory timestamps + auto & first = waypoints.front(); + rclcpp::Time referenceTime(first.header.stamp); + + // Compute IK for each waypoint + int ikAttemptsRemaining = options.ikAttempts; + for (size_t k = 0; k < waypoints.size(); k++) + { + auto & pose = waypoints[k]; + + // Create IK request + auto req = std::make_shared(); + req->ik_request.ik_link_name = tipLink; + req->ik_request.robot_state.joint_state.name = jointNames; + req->ik_request.robot_state.joint_state.position = jointPositions; + req->ik_request.group_name = groupName; + req->ik_request.avoid_collisions = options.avoidCollisions; + req->ik_request.pose_stamped = pose; + + RCLCPP_DEBUG( + getLogger(), "[CpJointSpaceTrajectoryPlanner] Computing IK for waypoint %lu/%lu", k + 1, + waypoints.size()); + + // Call IK service + auto resFuture = ikClient_->async_send_request(req); + auto status = resFuture.wait_for(options.ikTimeout); + + if (status != std::future_status::ready) + { + result.errorMessage = "IK service timeout at waypoint " + std::to_string(k); + RCLCPP_ERROR( + getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + result.errorCode = JointTrajectoryErrorCode::IK_SOLUTION_FAILED; + return result; + } + + auto res = resFuture.get(); + auto & prevTrajPoint = trajectory.back(); + + // Extract joint positions from IK solution + for (size_t j = 0; j < res->solution.joint_state.position.size(); j++) + { + auto & jointName = res->solution.joint_state.name[j]; + auto it = std::find(jointNames.begin(), jointNames.end(), jointName); + if (it != jointNames.end()) + { + int index = std::distance(jointNames.begin(), it); + jointPositions[index] = res->solution.joint_state.position[j]; + } + } + + // Continuity check + bool shouldCheck = k > 0 || !options.allowInitialDiscontinuity; + int discontinuityJointIndex = -1; + double discontinuityDelta = -1; + + if (shouldCheck) + { + for (size_t jointIndex = 0; jointIndex < jointPositions.size(); jointIndex++) + { + double deltaJoint = jointPositions[jointIndex] - prevTrajPoint[jointIndex]; + if (fabs(deltaJoint) > options.maxJointDiscontinuity) + { + discontinuityDelta = deltaJoint; + discontinuityJointIndex = jointIndex; + break; + } + } + } + + // Handle discontinuity + if (ikAttemptsRemaining > 0 && discontinuityJointIndex != -1) + { + // Retry IK + k--; + ikAttemptsRemaining--; + RCLCPP_WARN( + getLogger(), + "[CpJointSpaceTrajectoryPlanner] IK discontinuity detected, retrying (%d attempts " + "remaining)", + ikAttemptsRemaining); + continue; + } + else + { + // Record discontinuity or accept trajectory point + if (ikAttemptsRemaining == 0 && discontinuityJointIndex != -1) + { + result.discontinuityIndexes.push_back(k); + + RCLCPP_ERROR( + getLogger(), + "[CpJointSpaceTrajectoryPlanner] Joint discontinuity at waypoint %lu: joint '%s' " + "delta=%.3f rad", + k, jointNames[discontinuityJointIndex].c_str(), discontinuityDelta); + + if (k == 0) + { + RCLCPP_ERROR( + getLogger(), + "[CpJointSpaceTrajectoryPlanner] Initial posture discontinuity detected"); + } + } + + // Reset retry counter + ikAttemptsRemaining = options.ikAttempts; + + // Add point to trajectory + trajectory.push_back(jointPositions); + rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime; + trajectoryTimeStamps.push_back(durationFromStart); + } + } + + // Build RobotTrajectory message + result.trajectory.joint_trajectory.joint_names = jointNames; + + // Skip first point (current state) to avoid discontinuity in some behaviors + for (size_t i = 1; i < trajectory.size(); i++) + { + trajectory_msgs::msg::JointTrajectoryPoint jp; + jp.positions = trajectory[i]; + jp.time_from_start = trajectoryTimeStamps[i]; + result.trajectory.joint_trajectory.points.push_back(jp); + } + + // Determine success + if (!result.discontinuityIndexes.empty()) + { + if (result.discontinuityIndexes[0] == 0) + { + result.errorCode = JointTrajectoryErrorCode::INCORRECT_INITIAL_STATE; + result.errorMessage = "Incorrect initial state"; + } + else + { + result.errorCode = JointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY; + result.errorMessage = "Joint trajectory discontinuity detected"; + } + result.success = false; + RCLCPP_WARN(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + } + else + { + result.success = true; + result.errorCode = JointTrajectoryErrorCode::SUCCESS; + RCLCPP_INFO( + getLogger(), + "[CpJointSpaceTrajectoryPlanner] Successfully computed trajectory with %lu points", + result.trajectory.joint_trajectory.points.size()); + } + } + catch (const std::exception & e) + { + result.success = false; + result.errorMessage = std::string("Exception during trajectory planning: ") + e.what(); + RCLCPP_ERROR(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str()); + } + + return result; + } + +private: + ClMoveit2z * moveit2zClient_ = nullptr; + rclcpp::Client::SharedPtr ikClient_; +}; + +} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_motion_planner.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_motion_planner.hpp new file mode 100644 index 000000000..53de7d3dd --- /dev/null +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_motion_planner.hpp @@ -0,0 +1,371 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace cl_moveit2z +{ +/** + * @brief Configuration options for motion planning + */ +struct PlanningOptions +{ + std::optional planningTime; + std::optional maxVelocityScaling; + std::optional maxAccelerationScaling; + std::optional planningPipelineId; + std::optional plannerId; + std::optional poseReferenceFrame; +}; + +/** + * @brief Result of a planning operation + */ +struct PlanningResult +{ + bool success; + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveit::core::MoveItErrorCode errorCode; + std::string errorMessage; + + PlanningResult() + : success(false), errorCode(moveit::core::MoveItErrorCode::FAILURE), errorMessage("") + { + } +}; + +/** + * @brief Component for centralized motion planning operations + * + * This component wraps all MoveGroupInterface planning operations to provide + * a consistent interface for planning motions. It supports planning to: + * - Cartesian poses + * - Joint configurations + * - Named targets + * - Cartesian paths + * + * Pattern: API wrapper component with result types + */ +class CpMotionPlanner : public smacc2::ISmaccComponent +{ +public: + CpMotionPlanner() = default; + virtual ~CpMotionPlanner() = default; + + /** + * @brief Initialize the component and get reference to ClMoveit2z client + */ + inline void onInitialize() override + { + this->requiresClient(moveit2zClient_); + RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Component initialized"); + } + + /** + * @brief Plan to a Cartesian pose + * + * @param target Target pose (position and orientation) + * @param tipLink End effector tip link (optional, uses default if not specified) + * @param options Planning configuration options + * @return PlanningResult with success status and computed plan + */ + inline PlanningResult planToPose( + const geometry_msgs::msg::PoseStamped & target, + const std::optional & tipLink = std::nullopt, const PlanningOptions & options = {}) + { + PlanningResult result; + + if (!moveit2zClient_) + { + result.errorMessage = "ClMoveit2z client not available"; + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + return result; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + // Apply planning options + applyPlanningOptions(*moveGroup, options); + + // Set pose target + if (tipLink && !tipLink->empty()) + { + moveGroup->setPoseTarget(target, *tipLink); + } + else + { + moveGroup->setPoseTarget(target); + } + + // Set reference frame if specified + if (options.poseReferenceFrame) + { + moveGroup->setPoseReferenceFrame(*options.poseReferenceFrame); + } + else if (!target.header.frame_id.empty()) + { + moveGroup->setPoseReferenceFrame(target.header.frame_id); + } + + RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to pose..."); + + // Plan + result.errorCode = moveGroup->plan(result.plan); + result.success = (result.errorCode == moveit::core::MoveItErrorCode::SUCCESS); + + if (result.success) + { + RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to pose succeeded"); + } + else + { + result.errorMessage = + "Planning to pose failed with error code: " + std::to_string(result.errorCode.val); + RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + } + catch (const std::exception & e) + { + result.success = false; + result.errorMessage = std::string("Exception during planning: ") + e.what(); + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + + return result; + } + + /** + * @brief Plan to joint values + * + * @param jointTargets Map of joint names to target values + * @param options Planning configuration options + * @return PlanningResult with success status and computed plan + */ + inline PlanningResult planToJointTarget( + const std::map & jointTargets, const PlanningOptions & options = {}) + { + PlanningResult result; + + if (!moveit2zClient_) + { + result.errorMessage = "ClMoveit2z client not available"; + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + return result; + } + + if (jointTargets.empty()) + { + result.errorMessage = "No joint targets specified"; + RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + return result; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + // Apply planning options + applyPlanningOptions(*moveGroup, options); + + // Set joint value targets + moveGroup->setJointValueTarget(jointTargets); + + RCLCPP_INFO( + getLogger(), "[CpMotionPlanner] Planning to joint configuration (%lu joints)...", + jointTargets.size()); + + // Plan + result.errorCode = moveGroup->plan(result.plan); + result.success = (result.errorCode == moveit::core::MoveItErrorCode::SUCCESS); + + if (result.success) + { + RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to joint configuration succeeded"); + } + else + { + result.errorMessage = "Planning to joint configuration failed with error code: " + + std::to_string(result.errorCode.val); + RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + } + catch (const std::exception & e) + { + result.success = false; + result.errorMessage = std::string("Exception during planning: ") + e.what(); + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + + return result; + } + + /** + * @brief Plan a Cartesian path + * + * @param waypoints Vector of waypoint poses + * @param maxStep Maximum distance between path points (meters) + * @param jumpThreshold Maximum joint jump threshold (0.0 = disabled) + * @param avoidCollisions Whether to check for collisions + * @return PlanningResult with success status and computed trajectory + */ + inline PlanningResult planCartesianPath( + const std::vector & waypoints, double maxStep = 0.01, + double jumpThreshold = 0.0, bool avoidCollisions = true) + { + PlanningResult result; + + if (!moveit2zClient_) + { + result.errorMessage = "ClMoveit2z client not available"; + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + return result; + } + + if (waypoints.empty()) + { + result.errorMessage = "No waypoints specified"; + RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + return result; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + RCLCPP_INFO( + getLogger(), "[CpMotionPlanner] Planning Cartesian path with %lu waypoints...", + waypoints.size()); + + moveit_msgs::msg::RobotTrajectory trajectory; + double fractionAchieved = moveGroup->computeCartesianPath( + waypoints, maxStep, jumpThreshold, trajectory, avoidCollisions); + + result.plan.trajectory_ = trajectory; + + // Consider successful if we achieved at least 95% of the path + result.success = (fractionAchieved >= 0.95); + + if (result.success) + { + result.errorCode = moveit::core::MoveItErrorCode::SUCCESS; + RCLCPP_INFO( + getLogger(), "[CpMotionPlanner] Cartesian path planning succeeded (%.1f%% achieved)", + fractionAchieved * 100.0); + } + else + { + result.errorCode = moveit::core::MoveItErrorCode::PLANNING_FAILED; + result.errorMessage = "Cartesian path planning achieved only " + + std::to_string(fractionAchieved * 100.0) + "% of the path"; + RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + } + catch (const std::exception & e) + { + result.success = false; + result.errorMessage = std::string("Exception during Cartesian path planning: ") + e.what(); + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str()); + } + + return result; + } + + /** + * @brief Get current robot state + * + * @param waitTime Maximum time to wait for state (seconds) + * @return Pointer to current robot state, or nullptr if unavailable + */ + inline moveit::core::RobotStatePtr getCurrentState(double waitTime = 1.0) + { + if (!moveit2zClient_) + { + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] ClMoveit2z client not available"); + return nullptr; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + return moveGroup->getCurrentState(waitTime); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] Exception getting current state: %s", e.what()); + return nullptr; + } + } + +private: + ClMoveit2z * moveit2zClient_ = nullptr; + + /** + * @brief Apply planning options to MoveGroupInterface + * + * @param moveGroup Reference to MoveGroupInterface + * @param options Planning options to apply + */ + inline void applyPlanningOptions( + moveit::planning_interface::MoveGroupInterface & moveGroup, const PlanningOptions & options) + { + if (options.planningTime) + { + moveGroup.setPlanningTime(*options.planningTime); + } + + if (options.maxVelocityScaling) + { + moveGroup.setMaxVelocityScalingFactor(*options.maxVelocityScaling); + } + + if (options.maxAccelerationScaling) + { + moveGroup.setMaxAccelerationScalingFactor(*options.maxAccelerationScaling); + } + + if (options.planningPipelineId) + { + moveGroup.setPlanningPipelineId(*options.planningPipelineId); + } + + if (options.plannerId) + { + moveGroup.setPlannerId(*options.plannerId); + } + } +}; + +} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_tf_listener.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_tf_listener.hpp index acc37488d..d5427d935 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_tf_listener.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_tf_listener.hpp @@ -20,80 +20,174 @@ #pragma once -#include -#include +#include + +#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include namespace cl_moveit2z { -struct TfPoseTrack -{ - std::mutex mutex_; - geometry_msgs::msg::PoseStamped pose_; - std::string targetPoseFrame_; - std::string referenceBaseFrame_; - bool once = true; - bool isInitialized = false; -}; - -class CpTFListener : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable +/** + * @brief Component for shared TF2 transform management across all behaviors + * + * This component provides centralized access to TF2 transforms using static + * resource sharing to avoid creating multiple tf2_ros::Buffer and + * tf2_ros::TransformListener instances across different behaviors. + * + * Pattern: Static resource sharing (similar to nav2z_client's Pose component) + */ +class CpTfListener : public smacc2::ISmaccComponent { public: - CpTFListener(); + CpTfListener() = default; + virtual ~CpTfListener() = default; + + /** + * @brief Initialize the shared TF2 buffer and listener + * + * This method initializes the static tf2_ros::Buffer and tf2_ros::TransformListener + * on first use. Subsequent component instances will reuse the same resources. + */ + inline void onInitialize() override + { + std::lock_guard guard(tfMutex_); + + if (!tfBuffer_) + { + RCLCPP_INFO(getLogger(), "[CpTfListener] Initializing shared TF2 buffer and listener"); + tfBuffer_ = std::make_shared(getNode()->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + } + else + { + RCLCPP_DEBUG(getLogger(), "[CpTfListener] Reusing existing shared TF2 resources"); + } + } + + /** + * @brief Thread-safe transform lookup + * + * @param target_frame Target frame for the transform + * @param source_frame Source frame for the transform + * @param time Time at which to get the transform (default: latest available) + * @return std::optional Transform if available, std::nullopt otherwise + */ + inline std::optional lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Time & time = rclcpp::Time(0)) + { + std::lock_guard guard(tfMutex_); + + if (!tfBuffer_) + { + RCLCPP_ERROR( + getLogger(), "[CpTfListener] TF buffer not initialized. Call onInitialize() first."); + return std::nullopt; + } - virtual void update() override + try + { + auto transform = tfBuffer_->lookupTransform(target_frame, source_frame, time); + return transform; + } + catch (const tf2::TransformException & ex) + { + RCLCPP_WARN( + getLogger(), "[CpTfListener] Could not get transform from '%s' to '%s': %s", + source_frame.c_str(), target_frame.c_str(), ex.what()); + return std::nullopt; + } + } + + /** + * @brief Transform a pose to a different frame + * + * @param pose Input pose in its original frame + * @param target_frame Target frame to transform the pose into + * @return std::optional Transformed pose if successful, std::nullopt otherwise + */ + inline std::optional transformPose( + const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame) { - for (auto & poseTrack : poseTracks_) + std::lock_guard guard(tfMutex_); + + if (!tfBuffer_) + { + RCLCPP_ERROR( + getLogger(), "[CpTfListener] TF buffer not initialized. Call onInitialize() first."); + return std::nullopt; + } + + try { - tf2::Stamped transform; - try - { - { - std::lock_guard lock(listenerMutex_); - tfListener_->lookupTransform( - poseTrack->referenceBaseFrame_, poseTrack->targetPoseFrame_, rclcpp::Time(0), - transform); - } - - { - std::lock_guard guard(m_mutex_); - poseTrack->pose_.pose = tf2::toMsg(transform); - poseTrack->pose_.header.stamp = transform.stamp_; - poseTrack->pose_.header.frame_id = poseTrack->referenceBaseFrame_; - poseTrack->isInitialized = true; - } - } - catch (tf2::TransformException ex) - { - RCLCPP_ERROR_STREAM_THROTTLE( - 1, "[Component pose] (getLogger(), " << poseFrameName_ << "/[" << referenceFrame_ - << "] ) is failing on pose update : " << ex.what()); - } + geometry_msgs::msg::PoseStamped transformed_pose; + tfBuffer_->transform(pose, transformed_pose, target_frame); + return transformed_pose; + } + catch (const tf2::TransformException & ex) + { + RCLCPP_WARN( + getLogger(), "[CpTfListener] Could not transform pose to frame '%s': %s", + target_frame.c_str(), ex.what()); + return std::nullopt; } } - void getLastTransform( - std::string & targetPoseFrameName, std::string & referenceBaseFrame, - geometry_msgs::msg::Pose & out) + /** + * @brief Check if a transform is available + * + * @param target_frame Target frame + * @param source_frame Source frame + * @param time Time to check for (default: latest) + * @param timeout How long to wait for the transform + * @return bool True if transform is available, false otherwise + */ + inline bool canTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Time & time = rclcpp::Time(0), + const rclcpp::Duration & timeout = rclcpp::Duration::from_seconds(0.0)) { + std::lock_guard guard(tfMutex_); + + if (!tfBuffer_) + { + RCLCPP_ERROR( + getLogger(), "[CpTfListener] TF buffer not initialized. Call onInitialize() first."); + return false; + } + + return tfBuffer_->canTransform(target_frame, source_frame, time, timeout); } - std::future waitForNextTransform( - std::string & targetName, std::string & referenceBaseFrame) + /** + * @brief Get raw access to the TF2 buffer for advanced use cases + * + * WARNING: Direct buffer access bypasses thread safety. Use with caution. + * + * @return std::shared_ptr Shared pointer to the TF2 buffer + */ + inline std::shared_ptr getBuffer() { - tracks_ + std::lock_guard guard(tfMutex_); + return tfBuffer_; } private: + // Shared static resources across all CpTfListener instances + static std::shared_ptr tfBuffer_; static std::shared_ptr tfListener_; - static std::mutex listenerMutex_; - - std::mutex m_mutex_; - std::list> poseTracks_; + static std::mutex tfMutex_; }; + +// Static member initialization +std::shared_ptr CpTfListener::tfBuffer_ = nullptr; +std::shared_ptr CpTfListener::tfListener_ = nullptr; +std::mutex CpTfListener::tfMutex_; + } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_executor.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_executor.hpp new file mode 100644 index 000000000..e6179a7d7 --- /dev/null +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_executor.hpp @@ -0,0 +1,282 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace cl_moveit2z +{ +/** + * @brief Configuration options for trajectory execution + */ +struct ExecutionOptions +{ + std::optional maxVelocityScaling; + std::optional maxAccelerationScaling; + bool recordToHistory = true; + std::optional trajectoryName; +}; + +/** + * @brief Result of a trajectory execution operation + */ +struct ExecutionResult +{ + bool success; + moveit_msgs::msg::MoveItErrorCodes errorCode; + std::string errorMessage; + std::chrono::duration executionTime; + + ExecutionResult() : success(false), errorCode(), errorMessage(""), executionTime(0.0) + { + errorCode.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + } +}; + +/** + * @brief Component for centralized trajectory execution + * + * This component provides a unified interface for executing trajectories with: + * - Automatic recording to CpTrajectoryHistory + * - Consistent error handling and event posting + * - Execution options (velocity/acceleration scaling) + * - Execution time tracking + * + * Pattern: Executor with signals (like action clients) + */ +class CpTrajectoryExecutor : public smacc2::ISmaccComponent +{ +public: + CpTrajectoryExecutor() = default; + virtual ~CpTrajectoryExecutor() = default; + + /** + * @brief Initialize the component and get references to required components/clients + */ + inline void onInitialize() override + { + this->requiresClient(moveit2zClient_); + + // CpTrajectoryHistory is optional but recommended + this->requiresComponent(trajectoryHistory_, false); + + if (trajectoryHistory_) + { + RCLCPP_INFO( + getLogger(), + "[CpTrajectoryExecutor] Component initialized with trajectory history recording enabled"); + } + else + { + RCLCPP_WARN( + getLogger(), + "[CpTrajectoryExecutor] Component initialized without CpTrajectoryHistory " + "(trajectory history will not be recorded)"); + } + } + + /** + * @brief Execute a trajectory synchronously + * + * @param trajectory The robot trajectory to execute + * @param options Execution configuration options + * @return ExecutionResult with success status and error information + */ + inline ExecutionResult execute( + const moveit_msgs::msg::RobotTrajectory & trajectory, const ExecutionOptions & options = {}) + { + ExecutionResult result; + + if (!moveit2zClient_) + { + result.errorMessage = "ClMoveit2z client not available"; + RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str()); + return result; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + // Apply execution options + applyExecutionOptions(*moveGroup, options); + + RCLCPP_INFO( + getLogger(), "[CpTrajectoryExecutor] Executing trajectory with %lu points...", + trajectory.joint_trajectory.points.size()); + + // Track execution time + auto startTime = std::chrono::steady_clock::now(); + + // Execute trajectory + result.errorCode = moveGroup->execute(trajectory); + + auto endTime = std::chrono::steady_clock::now(); + result.executionTime = std::chrono::duration(endTime - startTime); + + result.success = (result.errorCode.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + + if (result.success) + { + RCLCPP_INFO( + getLogger(), "[CpTrajectoryExecutor] Trajectory execution succeeded (%.2f seconds)", + result.executionTime.count()); + } + else + { + result.errorMessage = + "Trajectory execution failed with error code: " + std::to_string(result.errorCode.val); + RCLCPP_WARN(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str()); + } + + // Record to history if enabled + if (options.recordToHistory && trajectoryHistory_) + { + std::string trajName = options.trajectoryName.value_or("unnamed_trajectory"); + recordToHistory(trajectory, result.errorCode, trajName); + } + } + catch (const std::exception & e) + { + result.success = false; + result.errorMessage = std::string("Exception during trajectory execution: ") + e.what(); + RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str()); + } + + return result; + } + + /** + * @brief Execute a motion plan synchronously + * + * Convenience method that accepts a MoveGroupInterface::Plan directly + * + * @param plan The motion plan containing the trajectory + * @param options Execution configuration options + * @return ExecutionResult with success status and error information + */ + inline ExecutionResult executePlan( + const moveit::planning_interface::MoveGroupInterface::Plan & plan, + const ExecutionOptions & options = {}) + { + return execute(plan.trajectory_, options); + } + + /** + * @brief Cancel ongoing execution + * + * Note: This is a best-effort cancellation and may not immediately stop the robot + */ + inline void cancel() + { + if (!moveit2zClient_) + { + RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] Cannot cancel: client not available"); + return; + } + + auto & moveGroup = moveit2zClient_->moveGroupClientInterface; + + try + { + moveGroup->stop(); + RCLCPP_INFO(getLogger(), "[CpTrajectoryExecutor] Trajectory execution cancelled"); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + getLogger(), "[CpTrajectoryExecutor] Exception during cancellation: %s", e.what()); + } + } + + /** + * @brief Get the trajectory history component + * + * @return Pointer to CpTrajectoryHistory, or nullptr if not available + */ + inline CpTrajectoryHistory * getTrajectoryHistory() { return trajectoryHistory_; } + +private: + ClMoveit2z * moveit2zClient_ = nullptr; + CpTrajectoryHistory * trajectoryHistory_ = nullptr; + + /** + * @brief Apply execution options to MoveGroupInterface + * + * @param moveGroup Reference to MoveGroupInterface + * @param options Execution options to apply + */ + inline void applyExecutionOptions( + moveit::planning_interface::MoveGroupInterface & moveGroup, const ExecutionOptions & options) + { + if (options.maxVelocityScaling) + { + moveGroup.setMaxVelocityScalingFactor(*options.maxVelocityScaling); + } + + if (options.maxAccelerationScaling) + { + moveGroup.setMaxAccelerationScalingFactor(*options.maxAccelerationScaling); + } + } + + /** + * @brief Record trajectory to history component + * + * @param trajectory The executed trajectory + * @param errorCode Execution result error code + * @param name Name for this trajectory entry + */ + inline void recordToHistory( + const moveit_msgs::msg::RobotTrajectory & trajectory, + const moveit_msgs::msg::MoveItErrorCodes & errorCode, const std::string & name) + { + if (!trajectoryHistory_) + { + return; + } + + try + { + trajectoryHistory_->pushTrajectory(name, trajectory, errorCode); + RCLCPP_DEBUG( + getLogger(), "[CpTrajectoryExecutor] Recorded trajectory '%s' to history", name.c_str()); + } + catch (const std::exception & e) + { + RCLCPP_WARN( + getLogger(), "[CpTrajectoryExecutor] Failed to record trajectory to history: %s", e.what()); + } + } +}; + +} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_history.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_history.hpp index 338f05510..4d1c1e266 100644 --- a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_history.hpp +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_history.hpp @@ -37,13 +37,54 @@ struct TrajectoryHistoryEntry class CpTrajectoryHistory : public smacc2::ISmaccComponent { public: - bool getLastTrajectory(int backIndex, moveit_msgs::msg::RobotTrajectory & trajectory); + inline bool getLastTrajectory(int backIndex, moveit_msgs::msg::RobotTrajectory & trajectory) + { + if (trajectoryHistory_.size() == 0) + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() << "] requested index: " << backIndex + << ", history size: " << trajectoryHistory_.size()); + return false; + } - bool getLastTrajectory(moveit_msgs::msg::RobotTrajectory & trajectory); + if (backIndex < 0) + { + backIndex = 0; + } + else if ((size_t)backIndex >= this->trajectoryHistory_.size()) + { + RCLCPP_WARN_STREAM( + getLogger(), "[" << getName() << "] requested index: " << backIndex + << ", history size: " << trajectoryHistory_.size()); + return false; + } - void pushTrajectory( + trajectory = + this->trajectoryHistory_[this->trajectoryHistory_.size() - 1 - backIndex].trajectory; + return true; + } + + inline bool getLastTrajectory(moveit_msgs::msg::RobotTrajectory & trajectory) + { + return getLastTrajectory(-1, trajectory); + } + + inline void pushTrajectory( std::string name, const moveit_msgs::msg::RobotTrajectory & trajectory, - moveit_msgs::msg::MoveItErrorCodes result); + moveit_msgs::msg::MoveItErrorCodes result) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] adding a new trajectory to the history ( " + << trajectory.joint_trajectory.points.size() << " poses)"); + + TrajectoryHistoryEntry entry; + this->trajectoryHistory_.push_back(entry); + + auto & last = this->trajectoryHistory_.back(); + last.trajectory = trajectory; + last.result = result; + last.name = name; + } private: std::vector trajectoryHistory_; diff --git a/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_visualizer.hpp b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_visualizer.hpp new file mode 100644 index 000000000..bd71f57a0 --- /dev/null +++ b/smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_trajectory_visualizer.hpp @@ -0,0 +1,245 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace cl_moveit2z +{ +/** + * @brief Component for visualizing trajectories as RViz markers + * + * This component publishes trajectory paths as visualization markers that can be + * viewed in RViz. It follows the ISmaccUpdatable pattern for periodic publishing. + * + * Pattern: Publisher + ISmaccUpdatable (similar to nav2z_client's CpWaypointsVisualizer) + */ +class CpTrajectoryVisualizer : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable +{ +public: + /** + * @brief Constructor + * + * @param publishRate Publishing rate (default: 10Hz) + */ + CpTrajectoryVisualizer(double publishRate = 10.0) : publishRate_(publishRate), enabled_(false) {} + + virtual ~CpTrajectoryVisualizer() = default; + + /** + * @brief Initialize the marker publisher + */ + inline void onInitialize() override + { + markersPub_ = + getNode()->create_publisher("trajectory_markers", 1); + RCLCPP_INFO(getLogger(), "[CpTrajectoryVisualizer] Initialized"); + + // Initialize timing for publish rate control + lastPublishTime_ = getNode()->now(); + } + + /** + * @brief Periodic update - publishes markers if enabled + */ + inline void update() override + { + std::lock_guard guard(markersMutex_); + + if (!enabled_ || !markersReady_) + { + return; + } + + // Rate limiting + auto now = getNode()->now(); + auto elapsed = (now - lastPublishTime_).seconds(); + if (elapsed < (1.0 / publishRate_)) + { + return; + } + + markersPub_->publish(markers_); + lastPublishTime_ = now; + } + + /** + * @brief Set trajectory to visualize + * + * @param poses Vector of poses defining the trajectory path + * @param ns Namespace for the markers (default: "trajectory") + */ + inline void setTrajectory( + const std::vector & poses, + const std::string & ns = "trajectory") + { + std::lock_guard guard(markersMutex_); + + RCLCPP_INFO( + getLogger(), "[CpTrajectoryVisualizer] Setting trajectory with %lu poses", poses.size()); + + markers_.markers.clear(); + + if (poses.empty()) + { + RCLCPP_WARN(getLogger(), "[CpTrajectoryVisualizer] Empty trajectory provided"); + markersReady_ = false; + return; + } + + // Create spheres for each pose + for (size_t i = 0; i < poses.size(); ++i) + { + visualization_msgs::msg::Marker marker; + marker.header = poses[i].header; + marker.ns = ns; + marker.id = static_cast(i); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose = poses[i].pose; + + marker.scale.x = markerScale_; + marker.scale.y = markerScale_; + marker.scale.z = markerScale_; + + marker.color.r = markerColor_[0]; + marker.color.g = markerColor_[1]; + marker.color.b = markerColor_[2]; + marker.color.a = markerColor_[3]; + + markers_.markers.push_back(marker); + } + + // Create line strip connecting all poses + if (poses.size() > 1) + { + visualization_msgs::msg::Marker lineMarker; + lineMarker.header = poses[0].header; + lineMarker.ns = ns + "_path"; + lineMarker.id = static_cast(poses.size()); + lineMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; + lineMarker.action = visualization_msgs::msg::Marker::ADD; + + lineMarker.scale.x = markerScale_ * 0.3; // Line width + + lineMarker.color.r = markerColor_[0]; + lineMarker.color.g = markerColor_[1]; + lineMarker.color.b = markerColor_[2]; + lineMarker.color.a = markerColor_[3] * 0.7; // Slightly transparent + + for (const auto & pose : poses) + { + lineMarker.points.push_back(pose.pose.position); + } + + markers_.markers.push_back(lineMarker); + } + + markersReady_ = true; + enabled_ = true; + + RCLCPP_INFO( + getLogger(), "[CpTrajectoryVisualizer] Created %lu markers", markers_.markers.size()); + } + + /** + * @brief Clear all markers + */ + inline void clearMarkers() + { + std::lock_guard guard(markersMutex_); + + RCLCPP_INFO(getLogger(), "[CpTrajectoryVisualizer] Clearing markers"); + + // Send delete action for all markers + for (auto & marker : markers_.markers) + { + marker.action = visualization_msgs::msg::Marker::DELETE; + } + + if (!markers_.markers.empty()) + { + markersPub_->publish(markers_); + } + + markers_.markers.clear(); + markersReady_ = false; + enabled_ = false; + } + + /** + * @brief Set marker color (RGBA values 0.0-1.0) + */ + inline void setColor(float r, float g, float b, float a = 1.0) + { + markerColor_[0] = r; + markerColor_[1] = g; + markerColor_[2] = b; + markerColor_[3] = a; + } + + /** + * @brief Set marker scale (sphere diameter in meters) + */ + inline void setScale(double scale) { markerScale_ = scale; } + + /** + * @brief Enable/disable marker publishing + */ + inline void setEnabled(bool enabled) + { + std::lock_guard guard(markersMutex_); + enabled_ = enabled; + } + + /** + * @brief Check if visualizer is enabled + */ + inline bool isEnabled() const { return enabled_; } + +private: + rclcpp::Publisher::SharedPtr markersPub_; + visualization_msgs::msg::MarkerArray markers_; + std::mutex markersMutex_; + + bool markersReady_ = false; + bool enabled_ = false; + + // Marker appearance + double markerScale_ = 0.02; // 2cm spheres + float markerColor_[4] = {0.0f, 1.0f, 0.0f, 0.8f}; // Green, slightly transparent + + // Publishing rate control + double publishRate_; + rclcpp::Time lastPublishTime_; +}; + +} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/cl_moveit2z.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/cl_moveit2z.cpp index 71f65bd73..b082bfa5e 100644 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/cl_moveit2z.cpp +++ b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/cl_moveit2z.cpp @@ -18,46 +18,19 @@ * ******************************************************************************************************************/ -#include -#include #include -#include - -using namespace std::chrono_literals; -using namespace moveit::planning_interface; namespace cl_moveit2z { +// Declare the Client's constructors. +ClMoveit2z::ClMoveit2z(std::string groupName) : options_(groupName) {} + ClMoveit2z::ClMoveit2z(const moveit::planning_interface::MoveGroupInterface::Options & options) : options_(options) { } -ClMoveit2z::ClMoveit2z(std::string groupName) : options_(groupName) {} - +// Declare the Client's destructor. ClMoveit2z::~ClMoveit2z() {} -void ClMoveit2z::onInitialize() -{ - moveGroupClientInterface = std::make_shared(getNode(), options_); - planningSceneInterface = std::make_shared(options_.move_group_namespace_); -} - -void ClMoveit2z::postEventMotionExecutionSucceded() -{ - RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Success Event"); - postEventMotionExecutionSucceded_(); -} - -void ClMoveit2z::postEventMotionExecutionFailed() -{ - RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Failure Event"); - postEventMotionExecutionFailed_(); -} - -const moveit::planning_interface::MoveGroupInterface::Options & ClMoveit2z::getOptions() const -{ - return options_; -} - } // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_attach_object.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_attach_object.cpp deleted file mode 100644 index 30e670315..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_attach_object.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -CbAttachObject::CbAttachObject(std::string targetObjectName) : targetObjectName_(targetObjectName) -{ -} - -CbAttachObject::CbAttachObject() {} - -void CbAttachObject::onEntry() -{ - cl_moveit2z::ClMoveit2z * moveGroup; - this->requiresClient(moveGroup); - - cl_moveit2z::CpGraspingComponent * graspingComponent; - this->requiresComponent(graspingComponent); - - // auto cubepos = cubeinfo->pose_->toPoseStampedMsg(); - - moveit_msgs::msg::CollisionObject targetCollisionObject; - - bool found = graspingComponent->getGraspingObject(targetObjectName_, targetCollisionObject); - - if (found) - { - targetCollisionObject.operation = moveit_msgs::msg::CollisionObject::ADD; - targetCollisionObject.header.stamp = getNode()->now(); - - moveGroup->planningSceneInterface->applyCollisionObject(targetCollisionObject); - // collisionObjects.push_back(cubeCollision); - - graspingComponent->currentAttachedObjectName = targetObjectName_; - moveGroup->moveGroupClientInterface->attachObject( - targetObjectName_, graspingComponent->gripperLink_, graspingComponent->fingerTipNames); - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Grasping objectfound. attach request."); - - this->postSuccessEvent(); - } - else - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << getName() << "] Grasping object was not found. Ignoring attach request."); - - this->postFailureEvent(); - } -} - -void CbAttachObject::onExit() {} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp deleted file mode 100644 index b674c1403..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include - -using namespace std::chrono_literals; - -namespace cl_moveit2z -{ -CbCircularPivotMotion::CbCircularPivotMotion(std::optional tipLink) -: CbMoveEndEffectorTrajectory(tipLink) -{ -} - -CbCircularPivotMotion::CbCircularPivotMotion( - const geometry_msgs::msg::PoseStamped & planePivotPose, double deltaRadians, - std::optional tipLink) -: CbMoveEndEffectorTrajectory(tipLink), planePivotPose_(planePivotPose), deltaRadians_(deltaRadians) -{ - if (tipLink_) planePivotPose_.header.frame_id = *tipLink; -} - -CbCircularPivotMotion::CbCircularPivotMotion( - const geometry_msgs::msg::PoseStamped & planePivotPose, - const geometry_msgs::msg::Pose & relativeInitialPose, double deltaRadians, - std::optional tipLink) -: CbMoveEndEffectorTrajectory(tipLink), - relativeInitialPose_(relativeInitialPose), - planePivotPose_(planePivotPose), - deltaRadians_(deltaRadians) -{ -} - -void CbCircularPivotMotion::generateTrajectory() -{ - if (!relativeInitialPose_) - { - this->computeCurrentEndEffectorPoseRelativeToPivot(); - } - - // project offset into the xy-plane - // get the radius - double radius = sqrt( - relativeInitialPose_->position.z * relativeInitialPose_->position.z + - relativeInitialPose_->position.y * relativeInitialPose_->position.y); - double initialAngle = atan2(relativeInitialPose_->position.z, relativeInitialPose_->position.y); - - double totallineardist = fabs(radius * deltaRadians_); - double totalangulardist = fabs(deltaRadians_); - - // at least 1 sample per centimeter (average) - // at least 1 sample per ~1.1 degrees (average) - - const double RADS_PER_SAMPLE = 0.02; - const double METERS_PER_SAMPLE = 0.01; - - int totalSamplesCount = - std::max(totallineardist / METERS_PER_SAMPLE, totalangulardist / RADS_PER_SAMPLE); - - double linearSecondsPerSample; - double angularSecondsPerSamples; - double secondsPerSample; - - if (linearSpeed_m_s_) - { - linearSecondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_); - } - else - { - linearSecondsPerSample = std::numeric_limits::max(); - } - - if (angularSpeed_rad_s_) - { - angularSecondsPerSamples = RADS_PER_SAMPLE / (*angularSpeed_rad_s_); - } - else - { - angularSecondsPerSamples = std::numeric_limits::max(); - } - - if (!linearSpeed_m_s_ && !angularSpeed_rad_s_) - { - secondsPerSample = 0.5; - } - else - { - secondsPerSample = std::min(linearSecondsPerSample, angularSecondsPerSamples); - } - - double currentAngle = initialAngle; - - double angleStep = deltaRadians_ / (double)totalSamplesCount; - - tf2::Transform tfBasePose; - tf2::fromMsg(planePivotPose_.pose, tfBasePose); - - RCLCPP_INFO_STREAM( - getLogger(), - "[" << getName() << "] generated trajectory, total samples: " << totalSamplesCount); - for (int i = 0; i < totalSamplesCount; i++) - { - // relativePose i - double y = radius * cos(currentAngle); - double z = radius * sin(currentAngle); - - geometry_msgs::msg::Pose relativeCurrentPose; - - relativeCurrentPose.position.x = relativeInitialPose_->position.x; - relativeCurrentPose.position.y = y; - relativeCurrentPose.position.z = z; - - tf2::Quaternion localquat; - localquat.setEuler(currentAngle, 0, 0); - - //relativeCurrentPose.orientation = relativeInitialPose_.orientation; - //tf2::toMsg(localquat, relativeCurrentPose.orientation); - relativeCurrentPose.orientation.w = 1; - - tf2::Transform tfRelativeCurrentPose; - tf2::fromMsg(relativeCurrentPose, tfRelativeCurrentPose); - - tf2::Transform tfGlobalPose = tfRelativeCurrentPose * tfBasePose; - - tfGlobalPose.setRotation(tfGlobalPose.getRotation() * localquat); - - geometry_msgs::msg::PoseStamped globalPose; - tf2::toMsg(tfGlobalPose, globalPose.pose); - globalPose.header.frame_id = planePivotPose_.header.frame_id; - globalPose.header.stamp = rclcpp::Time(planePivotPose_.header.stamp) + - rclcpp::Duration::from_seconds(i * secondsPerSample); - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "]" << rclcpp::Time(globalPose.header.stamp).nanoseconds()); - - this->endEffectorTrajectory_.push_back(globalPose); - currentAngle += angleStep; - } -} - -void CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot() -{ - //auto currentRobotEndEffectorPose = this->movegroupClient_->moveGroupClientInterface.getCurrentPose(); - - tf2_ros::Buffer tfBuffer(getNode()->get_clock()); - tf2_ros::TransformListener tfListener(tfBuffer); - - // tf2::Stamped globalBaseLink; - tf2::Stamped endEffectorInPivotFrame; - - try - { - if (!tipLink_ || *tipLink_ == "") - { - tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); - } - - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] waiting transform, pivot: '" - << planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'"); - tf2::fromMsg( - tfBuffer.lookupTransform( - planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)), - endEffectorInPivotFrame); - - //endEffectorInPivotFrame = tfBuffer.lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(0)); - - // we define here the global frame as the pivot frame id - // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10)); - // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink); - } - catch (const std::exception & e) - { - std::cerr << e.what() << '\n'; - } - - // tf2::Transform endEffectorInBaseLinkFrame; - // tf2::fromMsg(currentRobotEndEffectorPose.pose, endEffectorInBaseLinkFrame); - - // tf2::Transform endEffectorInPivotFrame = globalBaseLink * endEffectorInBaseLinkFrame; // pose composition - - // now pivot and EndEffector share a common reference frame (let say map) - // now get the current pose from the pivot reference frame with inverse composition - tf2::Transform pivotTransform; - tf2::fromMsg(planePivotPose_.pose, pivotTransform); - tf2::Transform invertedNewReferenceFrame = pivotTransform.inverse(); - - tf2::Transform currentPoseRelativeToPivot = invertedNewReferenceFrame * endEffectorInPivotFrame; - - geometry_msgs::msg::Pose finalEndEffectorRelativePose; - tf2::toMsg(currentPoseRelativeToPivot, finalEndEffectorRelativePose); - relativeInitialPose_ = finalEndEffectorRelativePose; -} - -void CbCircularPivotMotion::createMarkers() -{ - CbMoveEndEffectorTrajectory::createMarkers(); - - tf2::Transform localdirection; - localdirection.setIdentity(); - localdirection.setOrigin(tf2::Vector3(0.12, 0, 0)); - auto frameid = planePivotPose_.header.frame_id; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frameid; - marker.header.stamp = getNode()->now(); - marker.ns = "trajectory"; - marker.id = beahiorMarkers_.markers.size(); - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.01; - marker.scale.y = 0.02; - marker.scale.z = 0.02; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 0; - marker.color.b = 1.0; - - geometry_msgs::msg::Point start, end; - start.x = planePivotPose_.pose.position.x; - start.y = planePivotPose_.pose.position.y; - start.z = planePivotPose_.pose.position.z; - - tf2::Transform basetransform; - tf2::fromMsg(planePivotPose_.pose, basetransform); - tf2::Transform endarrow = localdirection * basetransform; - - end.x = endarrow.getOrigin().x(); - end.y = endarrow.getOrigin().y(); - end.z = endarrow.getOrigin().z(); - - marker.pose.orientation.w = 1; - marker.points.push_back(start); - marker.points.push_back(end); - - beahiorMarkers_.markers.push_back(marker); -} - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_detach_object.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_detach_object.cpp deleted file mode 100644 index 18cb95319..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_detach_object.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -void CbDetachObject::onEntry() -{ - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] requesting components"); - - cl_moveit2z::CpGraspingComponent * graspingComponent; - this->requiresComponent(graspingComponent); - - cl_moveit2z::ClMoveit2z * moveGroupClient; - this->requiresClient(moveGroupClient); - - auto & planningSceneInterface = moveGroupClient->planningSceneInterface; - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] requesting detach object"); - - auto res = moveGroupClient->moveGroupClientInterface->detachObject( - *(graspingComponent->currentAttachedObjectName)); - planningSceneInterface->removeCollisionObjects({*(graspingComponent->currentAttachedObjectName)}); - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] detach result: " << res); - - if (res) - this->postSuccessEvent(); - else - this->postFailureEvent(); -} - -void CbDetachObject::onExit() {} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp deleted file mode 100644 index a9b79e4f5..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include -#include - -#include - -#include - -using namespace std::chrono_literals; - -namespace cl_moveit2z -{ -CbEndEffectorRotate::CbEndEffectorRotate(double deltaRadians, std::optional tipLink) -: CbCircularPivotMotion(tipLink) -{ - deltaRadians_ = deltaRadians; -} - -CbEndEffectorRotate::~CbEndEffectorRotate() {} - -void CbEndEffectorRotate::onEntry() -{ - // autocompute pivot pose - tf2_ros::Buffer tfBuffer(getNode()->get_clock()); - tf2_ros::TransformListener tfListener(tfBuffer); - - tf2::Stamped endEffectorInPivotFrame; - - int attempts = 3; - - this->requiresClient(movegroupClient_); - if (!tipLink_) - { - tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); - RCLCPP_WARN_STREAM( - getLogger(), - "[" << getName() << "] tip unspecified, using default end effector: " << *tipLink_); - } - - while (attempts > 0) - { - try - { - //auto pivotFrameName = this->movegroupClient_->moveGroupClientInterface->getPlanningFrame(); - auto pivotFrameName = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); - - tf2::Stamped endEffectorInPivotFrame; - - tf2::fromMsg( - tfBuffer.lookupTransform(pivotFrameName, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)), - endEffectorInPivotFrame); - - tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose); - this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_; - this->planePivotPose_.header.stamp = - rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count()); - break; - } - catch (const std::exception & e) - { - RCLCPP_ERROR_STREAM(getLogger(), e.what() << ". Attempt countdown: " << attempts); - rclcpp::Duration(500ms); - attempts--; - } - } - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] pivotPose: " << planePivotPose_); - - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] calling base CbCircularPivotMotion::onEntry"); - CbCircularPivotMotion::onEntry(); -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp deleted file mode 100644 index b458a5d02..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -CbExecuteLastTrajectory::CbExecuteLastTrajectory() {} - -CbExecuteLastTrajectory::~CbExecuteLastTrajectory() {} - -void CbExecuteLastTrajectory::generateTrajectory() {} - -void CbExecuteLastTrajectory::onEntry() -{ - this->requiresClient(movegroupClient_); - - CpTrajectoryHistory * trajectoryHistory; - this->requiresComponent(trajectoryHistory); - - // this->generateTrajectory(); - // endEffectorTrajectory_ = - - // if (this->endEffectorTrajectory_.size() == 0) - // { - // RCLCPP_WARN_STREAM(getLogger(), "[" << smacc2::demangleSymbol(typeid(*this).name()) << "] No points in the trajectory. Skipping behavior."); - // return; - // } - - //this->createMarkers(); - //markersInitialized_ = true; - - moveit_msgs::msg::RobotTrajectory trajectory; - - if (trajectoryHistory->getLastTrajectory(trajectory)) - { - this->executeJointSpaceTrajectory(trajectory); - } -} - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp deleted file mode 100644 index defa85cda..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include - -#include - -namespace cl_moveit2z -{ -CbMoveCartesianRelative::CbMoveCartesianRelative() {} - -CbMoveCartesianRelative::CbMoveCartesianRelative(geometry_msgs::msg::Vector3 offset) -: offset_(offset) -{ -} - -void CbMoveCartesianRelative::onEntry() -{ - this->requiresClient(moveGroupSmaccClient_); - - if (this->group_) - { - moveit::planning_interface::MoveGroupInterface move_group( - getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_))); - this->moveRelativeCartesian(&move_group, offset_); - } - else - { - this->moveRelativeCartesian(moveGroupSmaccClient_->moveGroupClientInterface.get(), offset_); - } -} - -void CbMoveCartesianRelative::onExit() {} - -// keeps the end efector orientation fixed -void CbMoveCartesianRelative::moveRelativeCartesian( - moveit::planning_interface::MoveGroupInterface * movegroupClient, - geometry_msgs::msg::Vector3 & offset) -{ - std::vector waypoints; - - // this one was working fine but the issue is that for relative motions it grows up on ABORT-State-Loop pattern. - // But, we need current pose because maybe setCurrentPose was not set by previous behaviors. The only solution would be - // distinguishing between the execution error and the planning error with no state change - //auto referenceStartPose = movegroupClient->getPoseTarget(); - auto referenceStartPose = movegroupClient->getCurrentPose(); - movegroupClient->setPoseReferenceFrame(referenceStartPose.header.frame_id); - - RCLCPP_INFO_STREAM( - getLogger(), "[CbMoveCartesianRelative] RELATIVE MOTION, SOURCE POSE: " << referenceStartPose); - RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Offset: " << offset); - - waypoints.push_back(referenceStartPose.pose); // up and out - - auto endPose = referenceStartPose.pose; - - endPose.position.x += offset.x; - endPose.position.y += offset.y; - endPose.position.z += offset.z; - - RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] DESTINY POSE: " << endPose); - - // target_pose2.position.x -= 0.15; - waypoints.push_back(endPose); // left - - movegroupClient->setPoseTarget(endPose); - - float scalinf = 0.5; - if (scalingFactor_) scalinf = *scalingFactor_; - - RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Using scaling factor: " << scalinf); - movegroupClient->setMaxVelocityScalingFactor(scalinf); - - moveit_msgs::msg::RobotTrajectory trajectory; - double fraction = movegroupClient->computeCartesianPath( - waypoints, - 0.01, // eef_step - 0.00, // jump_threshold - trajectory); - - moveit::core::MoveItErrorCode behaviorResult; - if (fraction != 1.0 || fraction == -1) - { - RCLCPP_WARN_STREAM( - getLogger(), - "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje. Execution skipped " - "because not 100% of cartesian motion: " - << fraction * 100 << "%"); - behaviorResult = moveit::core::MoveItErrorCode::PLANNING_FAILED; - } - else - { - RCLCPP_INFO_STREAM( - getLogger(), - "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje: " << fraction); - - moveit::planning_interface::MoveGroupInterface::Plan grasp_pose_plan; - - // grasp_pose_plan.start_state_ = *(moveGroupInterface.getCurrentState()); - grasp_pose_plan.trajectory_ = trajectory; - behaviorResult = movegroupClient->execute(grasp_pose_plan); - } - - if (behaviorResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - RCLCPP_INFO( - getLogger(), "[CbMoveCartesianRelative] relative motion execution succeeded: fraction %lf.", - fraction); - moveGroupSmaccClient_->postEventMotionExecutionSucceded(); - this->postSuccessEvent(); - } - else - { - movegroupClient->setPoseTarget( - referenceStartPose); // undo changes since we did not executed the motion - RCLCPP_INFO( - getLogger(), "[CbMoveCartesianRelative] relative motion execution failed: fraction %lf.", - fraction); - moveGroupSmaccClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp deleted file mode 100644 index 7b36fd637..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -using namespace std::chrono_literals; - -CbMoveCartesianRelative2::CbMoveCartesianRelative2(std::string referenceFrame, std::string tipLink) -: CbMoveEndEffectorTrajectory(tipLink) -{ - globalFrame_ = referenceFrame; -} - -CbMoveCartesianRelative2::CbMoveCartesianRelative2( - std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset) -: CbMoveEndEffectorTrajectory(tipLink) -{ - globalFrame_ = referenceFrame; - offset_ = offset; -} - -CbMoveCartesianRelative2::~CbMoveCartesianRelative2() {} - -void CbMoveCartesianRelative2::generateTrajectory() -{ - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] generating trajectory"); - // at least 1 sample per centimeter (average) - const double METERS_PER_SAMPLE = 0.001; - - float dist_meters = 0; - tf2::Vector3 voffset; - tf2::fromMsg(offset_, voffset); - - float totallineardist = voffset.length(); - - int totalSamplesCount = totallineardist / METERS_PER_SAMPLE; - int steps = totallineardist / METERS_PER_SAMPLE; - - float interpolation_factor_step = 1.0 / totalSamplesCount; - - double secondsPerSample; - - if (!linearSpeed_m_s_) - { - linearSpeed_m_s_ = 0.1; - } - - secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_); - - tf2::Stamped currentEndEffectorTransform; - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current end effector pose"); - this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform); - - tf2::Transform finalEndEffectorTransform = currentEndEffectorTransform; - finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset); - - float linc = totallineardist / steps; // METERS_PER_SAMPLE with sign - float interpolation_factor = 0; - - rclcpp::Time startTime = getNode()->now(); - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps); - - for (float i = 0; i < steps; i++) - { - interpolation_factor += interpolation_factor_step; - dist_meters += linc; - - tf2::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp( - finalEndEffectorTransform.getOrigin(), interpolation_factor); - - tf2::Transform pose; - pose.setOrigin(vi); - pose.setRotation(currentEndEffectorTransform.getRotation()); - - geometry_msgs::msg::PoseStamped pointerPose; - tf2::toMsg(pose, pointerPose.pose); - pointerPose.header.frame_id = globalFrame_; - pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample); - - this->endEffectorTrajectory_.push_back(pointerPose); - } - - RCLCPP_INFO_STREAM( - getLogger(), - "[" << getName() << "] Target End efector Pose: " << this->endEffectorTrajectory_.back()); -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp deleted file mode 100644 index 47e07a9fa..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -// #include -#include -#include - -#include -#include -#include - -using namespace std::chrono_literals; - -namespace cl_moveit2z -{ -CbMoveEndEffector::CbMoveEndEffector() {} - -CbMoveEndEffector::CbMoveEndEffector( - geometry_msgs::msg::PoseStamped target_pose, std::string tip_link) -: targetPose(target_pose) -{ - tip_link_ = tip_link; -} - -void CbMoveEndEffector::onEntry() -{ - this->requiresClient(movegroupClient_); - - if (this->group_) - { - RCLCPP_DEBUG( - getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector"); - moveit::planning_interface::MoveGroupInterface move_group(getNode(), *group_); - this->moveToAbsolutePose(move_group, targetPose); - RCLCPP_DEBUG(getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed"); - } - else - { - RCLCPP_DEBUG( - getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector"); - this->moveToAbsolutePose(*(movegroupClient_->moveGroupClientInterface), targetPose); - RCLCPP_DEBUG(getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed"); - } -} - -bool CbMoveEndEffector::moveToAbsolutePose( - moveit::planning_interface::MoveGroupInterface & moveGroupInterface, - geometry_msgs::msg::PoseStamped & targetObjectPose) -{ - // auto & planningSceneInterface = movegroupClient_->planningSceneInterface; - RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds"); - rclcpp::sleep_for(500ms); - - moveGroupInterface.setPlanningTime(1.0); - - RCLCPP_INFO_STREAM( - getLogger(), "[CbMoveEndEffector] Target End efector Pose: " << targetObjectPose); - - moveGroupInterface.setPoseTarget(targetObjectPose, tip_link_); - moveGroupInterface.setPoseReferenceFrame(targetObjectPose.header.frame_id); - - moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan; - bool success = - (moveGroupInterface.plan(computedMotionPlan) == moveit::core::MoveItErrorCode::SUCCESS); - RCLCPP_INFO( - getLogger(), "[CbMoveEndEffector] Success Visualizing plan 1 (pose goal) %s", - success ? "" : "FAILED"); - - if (success) - { - auto executionResult = moveGroupInterface.execute(computedMotionPlan); - - if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution succeeded"); - movegroupClient_->postEventMotionExecutionSucceded(); - this->postSuccessEvent(); - } - else - { - RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed"); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } - } - else - { - RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed"); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } - - RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds"); - rclcpp::sleep_for(500ms); - - return success; -} - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp deleted file mode 100644 index af7ebf1da..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include - -#include -#include -#include -#include - -#include - -namespace cl_moveit2z -{ -CbMoveEndEffectorRelative::CbMoveEndEffectorRelative() { transform_.rotation.w = 1; } - -CbMoveEndEffectorRelative::CbMoveEndEffectorRelative(geometry_msgs::msg::Transform transform) -: transform_(transform) -{ -} - -void CbMoveEndEffectorRelative::onEntry() -{ - RCLCPP_INFO_STREAM( - getLogger(), - "[CbMoveEndEffectorRelative] Transform end effector pose relative: " << transform_); - - this->requiresClient(movegroupClient_); - - if (this->group_) - { - moveit::planning_interface::MoveGroupInterface move_group( - getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_))); - this->moveRelative(move_group, this->transform_); - } - else - { - this->moveRelative(*movegroupClient_->moveGroupClientInterface, this->transform_); - } -} - -void CbMoveEndEffectorRelative::onExit() {} - -void CbMoveEndEffectorRelative::moveRelative( - moveit::planning_interface::MoveGroupInterface & moveGroupInterface, - geometry_msgs::msg::Transform & transformOffset) -{ - auto referenceStartPose = moveGroupInterface.getCurrentPose(); - tf2::Quaternion currentOrientation; - tf2::convert(referenceStartPose.pose.orientation, currentOrientation); - tf2::Quaternion desiredRelativeRotation; - - tf2::convert(transformOffset.rotation, desiredRelativeRotation); - - auto targetOrientation = desiredRelativeRotation * currentOrientation; - - geometry_msgs::msg::PoseStamped targetObjectPose = referenceStartPose; - - targetObjectPose.pose.orientation = tf2::toMsg(targetOrientation); - targetObjectPose.pose.position.x += transformOffset.translation.x; - targetObjectPose.pose.position.y += transformOffset.translation.y; - targetObjectPose.pose.position.z += transformOffset.translation.z; - - moveGroupInterface.setPlanningTime(1.0); - - RCLCPP_INFO_STREAM( - getLogger(), "[CbMoveEndEffectorRelative] Target End efector Pose: " << targetObjectPose); - - moveGroupInterface.setPoseTarget(targetObjectPose); - moveGroupInterface.setPoseReferenceFrame("map"); - - moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan; - bool success = - (moveGroupInterface.plan(computedMotionPlan) == moveit::core::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(getLogger(), "Success Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); - - if (success) - { - auto executionResult = moveGroupInterface.execute(computedMotionPlan); - - if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution succeeded"); - movegroupClient_->postEventMotionExecutionSucceded(); - this->postSuccessEvent(); - } - else - { - moveGroupInterface.setPoseTarget( - referenceStartPose); // undo to avoid abort-repeat state issues with this relative motion - RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution failed"); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } - } - else - { - moveGroupInterface.setPoseTarget( - referenceStartPose); //undo since we did not executed the motion - RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution failed"); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp deleted file mode 100644 index 4ad3c64e8..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp +++ /dev/null @@ -1,465 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -#include -#include -#include -#include - -#include - -using namespace std::chrono_literals; - -namespace cl_moveit2z -{ -CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory(std::optional tipLink) -: tipLink_(tipLink), markersInitialized_(false) -{ -} - -CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory( - const std::vector & endEffectorTrajectory, - std::optional tipLink) -: tipLink_(tipLink), endEffectorTrajectory_(endEffectorTrajectory), markersInitialized_(false) - -{ -} - -void CbMoveEndEffectorTrajectory::initializeROS() -{ - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] initializing ros"); - - auto nh = this->getNode(); - markersPub_ = nh->create_publisher( - "trajectory_markers", rclcpp::QoS(1)); - iksrv_ = nh->create_client("/compute_ik"); -} - -ComputeJointTrajectoryErrorCode CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory( - moveit_msgs::msg::RobotTrajectory & computedJointTrajectory) -{ - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current state.. waiting"); - - // get current robot state - auto currentState = movegroupClient_->moveGroupClientInterface->getCurrentState(100); - - // get the IK client - auto groupname = movegroupClient_->moveGroupClientInterface->getName(); - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting joint names"); - auto currentjointnames = currentState->getJointModelGroup(groupname)->getActiveJointModelNames(); - - if (!tipLink_ || *tipLink_ == "") - { - tipLink_ = movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); - } - - std::vector jointPositions; - currentState->copyJointGroupPositions(groupname, jointPositions); - - std::vector> trajectory; - std::vector trajectoryTimeStamps; - - trajectory.push_back(jointPositions); - trajectoryTimeStamps.push_back(rclcpp::Duration(0s)); - - auto & first = endEffectorTrajectory_.front(); - rclcpp::Time referenceTime(first.header.stamp); - - std::vector discontinuityIndexes; - - int ikAttempts = 4; - for (size_t k = 0; k < this->endEffectorTrajectory_.size(); k++) - { - auto & pose = this->endEffectorTrajectory_[k]; - auto req = std::make_shared(); - //req.ik_request.attempts = 20; - - req->ik_request.ik_link_name = *tipLink_; - req->ik_request.robot_state.joint_state.name = currentjointnames; - req->ik_request.robot_state.joint_state.position = jointPositions; - - req->ik_request.group_name = groupname; - req->ik_request.avoid_collisions = true; - - //pose.header.stamp = getNode()->now(); - req->ik_request.pose_stamped = pose; - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] IK request: " << k << " " << *req); - - auto resfut = iksrv_->async_send_request(req); - - auto status = resfut.wait_for(3s); - if (status == std::future_status::ready) - { - //if (rclcpp::spin_until_future_complete(getNode(), resfut) == rclcpp::FutureReturnCode::SUCCESS) - //{ - auto & prevtrajpoint = trajectory.back(); - //jointPositions.clear(); - - auto res = resfut.get(); - std::stringstream ss; - for (size_t j = 0; j < res->solution.joint_state.position.size(); j++) - { - auto & jointname = res->solution.joint_state.name[j]; - auto it = std::find(currentjointnames.begin(), currentjointnames.end(), jointname); - if (it != currentjointnames.end()) - { - int index = std::distance(currentjointnames.begin(), it); - jointPositions[index] = res->solution.joint_state.position[j]; - ss << jointname << "(" << index << "): " << jointPositions[index] << std::endl; - } - } - - // continuity check - size_t jointindex = 0; - int discontinuityJointIndex = -1; - double discontinuityDeltaJointIndex = -1; - double deltajoint; - - bool check = k > 0 || !allowInitialTrajectoryStateJointDiscontinuity_ || - (allowInitialTrajectoryStateJointDiscontinuity_ && - !(*allowInitialTrajectoryStateJointDiscontinuity_)); - if (check) - { - for (jointindex = 0; jointindex < jointPositions.size(); jointindex++) - { - deltajoint = jointPositions[jointindex] - prevtrajpoint[jointindex]; - - if (fabs(deltajoint) > 0.3 /*2.5 deg*/) - { - discontinuityDeltaJointIndex = deltajoint; - discontinuityJointIndex = jointindex; - } - } - } - - if (ikAttempts > 0 && discontinuityJointIndex != -1) - { - k--; - ikAttempts--; - continue; - } - else - { - bool discontinuity = false; - if (ikAttempts == 0) - { - discontinuityIndexes.push_back(k); - discontinuity = true; - } - - ikAttempts = 4; - - if (discontinuity && discontinuityJointIndex != -1) - { - // show a message and stop the trajectory generation && jointindex!= 7 || fabs(deltajoint) > 0.1 /*2.5 deg*/ && jointindex== 7 - std::stringstream ss; - ss << "Traj[" << k << "/" << endEffectorTrajectory_.size() << "] " - << currentjointnames[discontinuityJointIndex] - << " IK discontinuity : " << discontinuityDeltaJointIndex << std::endl - << "prev joint value: " << prevtrajpoint[discontinuityJointIndex] << std::endl - << "current joint value: " << jointPositions[discontinuityJointIndex] << std::endl; - - ss << std::endl; - for (size_t ji = 0; ji < jointPositions.size(); ji++) - { - ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl; - } - - for (size_t kindex = 0; kindex < trajectory.size(); kindex++) - { - ss << "[" << kindex << "]: " << trajectory[kindex][discontinuityJointIndex] - << std::endl; - } - - if (k == 0) - { - ss << "This is the first posture of the trajectory. Maybe the robot initial posture is " - "not coincident to the initial posture of the generated joint trajectory." - << std::endl; - } - - RCLCPP_ERROR_STREAM(getLogger(), ss.str()); - - trajectory.push_back(jointPositions); - rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime; - trajectoryTimeStamps.push_back(durationFromStart); - - continue; - } - else - { - trajectory.push_back(jointPositions); - rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime; - trajectoryTimeStamps.push_back(durationFromStart); - - RCLCPP_DEBUG_STREAM(getLogger(), "IK solution: " << res->solution.joint_state); - RCLCPP_DEBUG_STREAM(getLogger(), "trajpoint: " << std::endl << ss.str()); - } - } - } - else - { - RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] wrong IK call"); - } - } - - // interpolate speeds? - - // interpolate accelerations? - - // get current robot state - // fill plan message - // computedMotionPlan.start_state_.joint_state.name = currentjointnames; - // computedMotionPlan.start_state_.joint_state.position = trajectory.front(); - // computedMotionPlan.trajectory_.joint_trajectory.joint_names = currentjointnames; - - computedJointTrajectory.joint_trajectory.joint_names = currentjointnames; - int i = 0; - for (auto & p : trajectory) - { - if ( - i == - 0) // not copy the current state in the trajectory (used to solve discontinuity in other behaviors) - { - i++; - continue; - } - - trajectory_msgs::msg::JointTrajectoryPoint jp; - jp.positions = p; - jp.time_from_start = trajectoryTimeStamps[i]; //rclcpp::Duration(t); - computedJointTrajectory.joint_trajectory.points.push_back(jp); - i++; - } - - if (discontinuityIndexes.size()) - { - if (discontinuityIndexes[0] == 0) - return ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE; - else - return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY; - } - - return ComputeJointTrajectoryErrorCode::SUCCESS; -} - -void CbMoveEndEffectorTrajectory::executeJointSpaceTrajectory( - const moveit_msgs::msg::RobotTrajectory & computedJointTrajectory) -{ - RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory"); - // call execute - auto executionResult = - this->movegroupClient_->moveGroupClientInterface->execute(computedJointTrajectory); - - if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution succeeded"); - movegroupClient_->postEventMotionExecutionSucceded(); - this->postSuccessEvent(); - } - else - { - this->postMotionExecutionFailureEvents(); - this->postFailureEvent(); - } -} - -void CbMoveEndEffectorTrajectory::onEntry() -{ - this->requiresClient(movegroupClient_); - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Generating end effector trajectory"); - - this->generateTrajectory(); - - if (this->endEffectorTrajectory_.size() == 0) - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << smacc2::demangleSymbol(typeid(*this).name()) - << "] No points in the trajectory. Skipping behavior."); - return; - } - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating markers."); - - this->createMarkers(); - markersInitialized_ = true; - moveit_msgs::msg::RobotTrajectory computedTrajectory; - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Computing joint space trajectory."); - - auto errorcode = computeJointSpaceTrajectory(computedTrajectory); - - bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS; - - CpTrajectoryHistory * trajectoryHistory; - this->requiresComponent(trajectoryHistory); - - if (!trajectoryGenerationSuccess) - { - RCLCPP_INFO_STREAM( - getLogger(), "[" << this->getName() << "] Incorrect trajectory. Posting failure event."); - if (trajectoryHistory != nullptr) - { - moveit_msgs::msg::MoveItErrorCodes error; - error.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; - trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error); - } - - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - - if (errorcode == ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY) - { - this->postJointDiscontinuityEvent(computedTrajectory); - } - else if (errorcode == ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE) - { - this->postIncorrectInitialStateEvent(computedTrajectory); - } - return; - } - else - { - if (trajectoryHistory != nullptr) - { - moveit_msgs::msg::MoveItErrorCodes error; - error.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error); - } - - this->executeJointSpaceTrajectory(computedTrajectory); - } - - // handle finishing events -} // namespace cl_moveit2z - -void CbMoveEndEffectorTrajectory::update() -{ - if (markersInitialized_) - { - std::lock_guard guard(m_mutex_); - markersPub_->publish(beahiorMarkers_); - } -} - -void CbMoveEndEffectorTrajectory::onExit() -{ - markersInitialized_ = false; - - if (autocleanmarkers) - { - std::lock_guard guard(m_mutex_); - for (auto & marker : this->beahiorMarkers_.markers) - { - marker.header.stamp = getNode()->now(); - marker.action = visualization_msgs::msg::Marker::DELETE; - } - - markersPub_->publish(beahiorMarkers_); - } -} - -void CbMoveEndEffectorTrajectory::createMarkers() -{ - tf2::Transform localdirection; - localdirection.setIdentity(); - localdirection.setOrigin(tf2::Vector3(0.05, 0, 0)); - auto frameid = this->endEffectorTrajectory_.front().header.frame_id; - - for (auto & pose : this->endEffectorTrajectory_) - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frameid; - marker.header.stamp = getNode()->now(); - marker.ns = "trajectory"; - marker.id = this->beahiorMarkers_.markers.size(); - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.005; - marker.scale.y = 0.01; - marker.scale.z = 0.01; - marker.color.a = 0.8; - marker.color.r = 1.0; - marker.color.g = 0; - marker.color.b = 0; - - geometry_msgs::msg::Point start, end; - start.x = 0; - start.y = 0; - start.z = 0; - - tf2::Transform basetransform; - tf2::fromMsg(pose.pose, basetransform); - // tf2::Transform endarrow = localdirection * basetransform; - - end.x = localdirection.getOrigin().x(); - end.y = localdirection.getOrigin().y(); - end.z = localdirection.getOrigin().z(); - - marker.pose.position = pose.pose.position; - marker.pose.orientation = pose.pose.orientation; - marker.points.push_back(start); - marker.points.push_back(end); - - beahiorMarkers_.markers.push_back(marker); - } -} - -void CbMoveEndEffectorTrajectory::generateTrajectory() -{ - // bypass current trajectory, overridden in derived classes - // this->endEffectorTrajectory_ = ... -} - -void CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose( - std::string globalFrame, tf2::Stamped & currentEndEffectorTransform) -{ - tf2_ros::Buffer tfBuffer(getNode()->get_clock()); - tf2_ros::TransformListener tfListener(tfBuffer); - - try - { - if (!tipLink_ || *tipLink_ == "") - { - tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink(); - } - - tf2::fromMsg( - tfBuffer.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), rclcpp::Duration(10s)), - currentEndEffectorTransform); - - //tfListener.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), currentEndEffectorTransform); - - // we define here the global frame as the pivot frame id - // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10)); - // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink); - } - catch (const std::exception & e) - { - std::cerr << e.what() << std::endl; - } -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_joints.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_joints.cpp deleted file mode 100644 index 595e688ca..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_joints.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -CbMoveJoints::CbMoveJoints(const std::map & jointValueTarget) -: jointValueTarget_(jointValueTarget) -{ -} - -CbMoveJoints::CbMoveJoints() {} - -void CbMoveJoints::onEntry() -{ - this->requiresClient(movegroupClient_); - - if (this->group_) - { - moveit::planning_interface::MoveGroupInterface move_group( - getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_))); - this->moveJoints(move_group); - } - else - { - this->moveJoints(*movegroupClient_->moveGroupClientInterface); - } -} - -std::string currentJointStatesToString( - moveit::planning_interface::MoveGroupInterface & moveGroupInterface, - std::map & targetJoints) -{ - auto state = moveGroupInterface.getCurrentState(); - - if (state == nullptr) return std::string(); - - auto vnames = state->getVariableNames(); - - std::stringstream ss; - - for (auto & tgj : targetJoints) - { - auto it = std::find(vnames.begin(), vnames.end(), tgj.first); - auto index = std::distance(vnames.begin(), it); - - ss << tgj.first << ":" << state->getVariablePosition(index) << std::endl; - } - - return ss.str(); -} - -void CbMoveJoints::moveJoints(moveit::planning_interface::MoveGroupInterface & moveGroupInterface) -{ - if (scalingFactor_) moveGroupInterface.setMaxVelocityScalingFactor(*scalingFactor_); - - bool success; - moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan; - - if (jointValueTarget_.size() == 0) - { - RCLCPP_WARN( - getLogger(), "[CbMoveJoints] No joint was value specified. Skipping planning call."); - success = false; - } - else - { - moveGroupInterface.setJointValueTarget(jointValueTarget_); - //moveGroupInterface.setGoalJointTolerance(0.01); - - auto result = moveGroupInterface.plan(computedMotionPlan); - - success = (result == moveit::core::MoveItErrorCode::SUCCESS); - - RCLCPP_INFO( - getLogger(), "[CbMoveJoints] Execution plan result %s (%d)", success ? "SUCCESS" : "FAILED", - result.val); - } - - if (success) - { - auto executionResult = moveGroupInterface.execute(computedMotionPlan); - - //auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_); - - if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - RCLCPP_INFO_STREAM( - getLogger(), "[" << this->getName() - << "] motion execution succeeded. Throwing success event. " << std::endl - // << statestr - ); - movegroupClient_->postEventMotionExecutionSucceded(); - this->postSuccessEvent(); - } - else - { - RCLCPP_WARN_STREAM( - getLogger(), - "[" << this->getName() << "] motion execution failed. Throwing fail event." << std::endl - // << statestr - ); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } - } - else - { - auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_); - RCLCPP_WARN_STREAM( - getLogger(), - "[" << this->getName() << "] motion execution failed. Throwing fail event." << std::endl - // << statestr - ); - movegroupClient_->postEventMotionExecutionFailed(); - this->postFailureEvent(); - } -} - -void CbMoveJoints::onExit() {} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp deleted file mode 100644 index d01dd8ca4..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include -#include -#include - -namespace cl_moveit2z -{ -CbMoveKnownState::CbMoveKnownState(std::string pkg, std::string config_path) -: pkg_(pkg), config_path_(config_path) -{ -} - -CbMoveKnownState::~CbMoveKnownState() {} - -void CbMoveKnownState::onEntry() -{ - jointValueTarget_ = loadJointStatesFromFile(pkg_, config_path_); - CbMoveJoints::onEntry(); -} - -#define HAVE_NEW_YAMLCPP -std::map CbMoveKnownState::loadJointStatesFromFile( - std::string pkg, std::string filepath) -{ - //auto pkgpath = ros::package::getPath(pkg); - std::string pkgpath = ament_index_cpp::get_package_share_directory(pkg); - - std::map jointStates; - - if (pkgpath == "") - { - RCLCPP_ERROR_STREAM( - getLogger(), "[" << getName() << "] package not found for the known poses file: " << pkg - << std::endl - << " [IGNORING BEHAVIOR]"); - return jointStates; - } - - filepath = pkgpath + "/" + filepath; - - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] Opening file with joint known state: " << filepath); - - if (std::filesystem::exists(filepath)) - { - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] known state file exists: " << filepath); - } - else - { - RCLCPP_ERROR_STREAM( - getLogger(), "[" << getName() << "] known state file does not exists: " << filepath); - } - - std::ifstream ifs(filepath.c_str(), std::ifstream::in); - if (ifs.good() == false) - { - RCLCPP_ERROR_STREAM( - getLogger(), - "[" << getName() << "] Error opening file with joint known states: " << filepath); - throw std::string("joint state files not found"); - } - - try - { -#ifdef HAVE_NEW_YAMLCPP - YAML::Node node = YAML::Load(ifs); -#else - YAML::Parser parser(ifs); - parser.GetNextDocument(node); -#endif - -#ifdef HAVE_NEW_YAMLCPP - const YAML::Node & wp_node_tmp = node["joint_states"]; - const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL; -#else - const YAML::Node * wp_node = node.FindValue("waypoints"); -#endif - - if (wp_node != NULL) - { - try - { - for (YAML::const_iterator it = wp_node->begin(); it != wp_node->end(); ++it) - { - std::string key = it->first.as(); - double value = it->second.as(); - RCLCPP_DEBUG_STREAM(getLogger(), " joint - " << key << ": " << value); - jointStates[key] = value; - } - - return jointStates; - } - catch (std::exception & ex) - { - RCLCPP_ERROR(getLogger(), "trying to convert to map, failed, errormsg: %s", ex.what()); - } - - RCLCPP_INFO_STREAM(getLogger(), "Parsed " << jointStates.size() << " joint entries."); - } - else - { - RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any jointStates in the provided yaml file."); - } - } - catch (const YAML::ParserException & ex) - { - RCLCPP_ERROR_STREAM( - getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what()); - } - return jointStates; -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp deleted file mode 100644 index e859618a5..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -CbMoveLastTrajectoryInitialState::CbMoveLastTrajectoryInitialState() {} - -CbMoveLastTrajectoryInitialState::CbMoveLastTrajectoryInitialState(int backIndex) -: backIndex_(backIndex) -{ -} - -CbMoveLastTrajectoryInitialState::~CbMoveLastTrajectoryInitialState() {} - -void CbMoveLastTrajectoryInitialState::onEntry() -{ - CpTrajectoryHistory * trajectoryHistory; - this->requiresComponent(trajectoryHistory); - - if (trajectoryHistory != nullptr) - { - moveit_msgs::msg::RobotTrajectory trajectory; - - bool trajectoryFound = trajectoryHistory->getLastTrajectory(backIndex_, trajectory); - - if (trajectoryFound) - { - trajectory_msgs::msg::JointTrajectoryPoint & initialPoint = - trajectory.joint_trajectory.points.front(); - - std::stringstream ss; - for (size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); i++) - { - auto & name = trajectory.joint_trajectory.joint_names[i]; - - jointValueTarget_[name] = initialPoint.positions[i]; - ss << name << ": " << jointValueTarget_[name] << std::endl; - } - RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "]" << std::endl << ss.str()); - - RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry"); - CbMoveJoints::onEntry(); - RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry finished"); - } - } - - //call base OnEntry -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp deleted file mode 100644 index cb3057ebb..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include - -namespace cl_moveit2z -{ -CbMoveNamedTarget::CbMoveNamedTarget(std::string namedtarget) : namedTarget_(namedtarget) {} - -void CbMoveNamedTarget::onEntry() -{ - this->requiresClient(movegroupClient_); - movegroupClient_->moveGroupClientInterface->setNamedTarget(this->namedTarget_); -} - -void CbMoveNamedTarget::onExit() {} - -std::map CbMoveNamedTarget::getNamedTargetValues() -{ - return movegroupClient_->moveGroupClientInterface->getNamedTargetValues(this->namedTarget_); -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp deleted file mode 100644 index 5af9b9a01..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include - -namespace cl_moveit2z -{ -CbCircularPouringMotion::CbCircularPouringMotion( - const geometry_msgs::msg::Point & relativePivotPoint, double deltaHeight, std::string tipLink, - std::string globalFrame) -: CbMoveEndEffectorTrajectory(tipLink), - relativePivotPoint_(relativePivotPoint), - deltaHeight_(deltaHeight), - globalFrame_(globalFrame) - -{ -} - -void CbCircularPouringMotion::generateTrajectory() -{ - // at least 1 sample per centimeter (average) - const double METERS_PER_SAMPLE = 0.001; - - float dist_meters = 0; - float totallineardist = fabs(this->deltaHeight_); - - int totalSamplesCount = totallineardist / METERS_PER_SAMPLE; - int steps = totallineardist / METERS_PER_SAMPLE; - - float interpolation_factor_step = 1.0 / totalSamplesCount; - - double secondsPerSample; - - if (linearSpeed_m_s_) - { - secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_); - } - else - { - secondsPerSample = std::numeric_limits::max(); - } - - tf2::Stamped currentEndEffectorTransform; - - this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform); - - tf2::Transform lidEndEffectorTransform; - tf2::fromMsg(this->pointerRelativePose_, lidEndEffectorTransform); - - tf2::Vector3 v0, v1; - v0 = (currentEndEffectorTransform * lidEndEffectorTransform).getOrigin(); - - tf2::Vector3 pivotPoint; - - tf2::fromMsg(this->relativePivotPoint_, pivotPoint); - - tf2::Vector3 pivot = (currentEndEffectorTransform * pivotPoint); - - v0 = v0 - pivot; - v1 = v0; - v1.setZ(v1.z() + this->deltaHeight_); - - tf2::Vector3 vp1(v1); - tf2::Vector3 vp0(v0); - tf2::Quaternion rotation = tf2::shortestArcQuatNormalize2(vp0, vp1); - - tf2::Quaternion initialEndEffectorOrientation = currentEndEffectorTransform.getRotation(); - auto finalEndEffectorOrientation = initialEndEffectorOrientation * rotation; - - tf2::Quaternion initialPointerOrientation = - initialEndEffectorOrientation * lidEndEffectorTransform.getRotation(); - tf2::Quaternion finalPointerOrientation = - finalEndEffectorOrientation * lidEndEffectorTransform.getRotation(); - - // auto shortestAngle = - // tf2::angleShortestPath(initialEndEffectorOrientation, finalEndEffectorOrientation); - - v0 += pivot; - v1 += pivot; - - float linc = deltaHeight_ / steps; // METERS_PER_SAMPLE with sign - float interpolation_factor = 0; - tf2::Vector3 vi = v0; - - tf2::Transform invertedLidTransform = lidEndEffectorTransform.inverse(); - rclcpp::Time startTime = getNode()->now(); - - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps); - for (float i = 0; i < steps; i++) - { - // auto currentEndEffectorOrientation = - // tf2::slerp(initialEndEffectorOrientation, finalEndEffectorOrientation, interpolation_factor); - auto currentPointerOrientation = - tf2::slerp(initialPointerOrientation, finalPointerOrientation, interpolation_factor); - - interpolation_factor += interpolation_factor_step; - dist_meters += linc; - - vi = v0; - vi.setZ(vi.getZ() + dist_meters); - - tf2::Transform pose; - pose.setOrigin(vi); - pose.setRotation(currentPointerOrientation); - - geometry_msgs::msg::PoseStamped pointerPose; - tf2::toMsg(pose, pointerPose.pose); - pointerPose.header.frame_id = globalFrame_; - pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample); - this->pointerTrajectory_.push_back(pointerPose); - - tf2::Transform poseEndEffector = pose * invertedLidTransform; - - geometry_msgs::msg::PoseStamped globalEndEffectorPose; - tf2::toMsg(poseEndEffector, globalEndEffectorPose.pose); - globalEndEffectorPose.header.frame_id = globalFrame_; - globalEndEffectorPose.header.stamp = - startTime + rclcpp::Duration::from_seconds(i * secondsPerSample); - - this->endEffectorTrajectory_.push_back(globalEndEffectorPose); - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] " << i << " - " << globalEndEffectorPose); - } - RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "]trajectory generated, size: " << steps); -} - -void CbCircularPouringMotion::createMarkers() -{ - CbMoveEndEffectorTrajectory::createMarkers(); - - tf2::Stamped currentEndEffectorTransform; - this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform); - tf2::Vector3 pivotPoint; - - tf2::fromMsg(this->relativePivotPoint_, pivotPoint); - - tf2::Vector3 pivot = (currentEndEffectorTransform * pivotPoint); - - visualization_msgs::msg::Marker marker; - - marker.ns = "trajectory"; - marker.id = beahiorMarkers_.markers.size(); - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.01; - marker.scale.y = 0.01; - marker.scale.z = 0.01; - - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 0; - marker.color.b = 1.0; - - tf2::toMsg(pivot, marker.pose.position); - - marker.header.frame_id = globalFrame_; - marker.header.stamp = getNode()->now(); - - beahiorMarkers_.markers.push_back(marker); - - tf2::Transform localdirection; - localdirection.setIdentity(); - localdirection.setOrigin(tf2::Vector3(0.05, 0, 0)); - auto frameid = this->pointerTrajectory_.front().header.frame_id; - - for (auto & pose : this->pointerTrajectory_) - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frameid; - marker.header.stamp = getNode()->now(); - marker.ns = "trajectory"; - marker.id = this->beahiorMarkers_.markers.size(); - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.005; - marker.scale.y = 0.01; - marker.scale.z = 0.01; - marker.color.a = 0.8; - marker.color.r = 0.0; - marker.color.g = 1; - marker.color.b = 0.0; - - geometry_msgs::msg::Point start, end; - start.x = 0; - start.y = 0; - start.z = 0; - - tf2::Transform basetransform; - tf2::fromMsg(pose.pose, basetransform); - // tf2::Transform endarrow = localdirection * basetransform; - - end.x = localdirection.getOrigin().x(); - end.y = localdirection.getOrigin().y(); - end.z = localdirection.getOrigin().z(); - - marker.pose.position = pose.pose.position; - marker.pose.orientation = pose.pose.orientation; - marker.points.push_back(start); - marker.points.push_back(end); - - beahiorMarkers_.markers.push_back(marker); - } -} - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp deleted file mode 100644 index be6cc69e1..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include - -namespace cl_moveit2z -{ -CbUndoLastTrajectory::CbUndoLastTrajectory() {} - -CbUndoLastTrajectory::CbUndoLastTrajectory(int backIndex) : backIndex_(backIndex) {} - -CbUndoLastTrajectory::~CbUndoLastTrajectory() {} - -void CbUndoLastTrajectory::generateTrajectory() {} - -void CbUndoLastTrajectory::onEntry() -{ - CpTrajectoryHistory * trajectoryHistory; - this->requiresComponent(trajectoryHistory); - this->requiresClient(movegroupClient_); - - if (trajectoryHistory->getLastTrajectory(backIndex_, trajectory)) - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << getName() << "] reversing last trajectory [" << backIndex_ << "]"); - - auto initialTime = trajectory.joint_trajectory.points.back().time_from_start; - - reversed = trajectory; - - std::reverse(reversed.joint_trajectory.points.begin(), reversed.joint_trajectory.points.end()); - - for (auto & jp : reversed.joint_trajectory.points) - { - jp.time_from_start = rclcpp::Duration(initialTime) - rclcpp::Duration(jp.time_from_start); - } - - this->executeJointSpaceTrajectory(reversed); - } - else - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << getName() << "] could not undo last trajectory, trajectory not found."); - } -} - -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/common.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/common.cpp deleted file mode 100644 index 20d82520b..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/common.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include -#include -#include -#include - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg) -{ - return out << " Quaternion[" << msg.x << " , " << msg.y << " , " << msg.z << ", w:" << msg.w; -} - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Transform & msg) -{ - return out << "Translation[" << msg.translation << "], Rotation[" << msg.rotation << "]"; -} - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg) -{ - return out << "Position[" << msg.position << "], Orientation[" << msg.orientation << "]"; -} - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg) -{ - return out << "[serialization geometry_msgs::msg::PoseStamped] frame_id: " << msg.header.frame_id - << ", pose: " << msg.pose; -} - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Vector3 & msg) -{ - return out << "[ " << msg.x << " " << msg.y << " " << msg.z << "]"; -} - -std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg) -{ - return out << "[ " << msg.x << " " << msg.y << " " << msg.z << "]"; -} - -std::ostream & operator<<(std::ostream & out, const moveit_msgs::srv::GetPositionIK::Request & msg) -{ - return out << "[moveit_msgs::srv::GetPositionIK::Request] position[" - << msg.ik_request.pose_stamped << "]"; -} - -std::ostream & operator<<(std::ostream & out, const sensor_msgs::msg::JointState & /*msg*/) -{ - return out << "[sensor_msgs::msg::JointState]"; -} diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_grasping_objects.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_grasping_objects.cpp deleted file mode 100644 index 8cc0c2649..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_grasping_objects.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include - -namespace cl_moveit2z -{ -bool CpGraspingComponent::getGraspingObject( - std::string name, moveit_msgs::msg::CollisionObject & object) -{ - if (this->graspingObjects.count(name)) - { - object = this->graspingObjects[name]; - return true; - } - else - { - return false; - } -} - -void CpGraspingComponent::createGraspableBox( - std::string frameid, float x, float y, float z, float xl, float yl, float zl) -{ - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] creating grasping object in planning scene: " << frameid); - moveit_msgs::msg::CollisionObject collision; - auto boxname = frameid; - ; - collision.id = boxname; - collision.header.frame_id = frameid; - - collision.primitives.resize(1); - collision.primitives[0].type = collision.primitives[0].BOX; - collision.primitives[0].dimensions.resize(3); - - collision.primitives[0].dimensions[0] = xl; - collision.primitives[0].dimensions[1] = yl; - collision.primitives[0].dimensions[2] = zl; - - collision.primitive_poses.resize(1); - collision.primitive_poses[0].position.x = x; - collision.primitive_poses[0].position.y = y; - collision.primitive_poses[0].position.z = z; - collision.primitive_poses[0].orientation.w = 1.0; - - graspingObjects[boxname] = collision; -} -} // namespace cl_moveit2z diff --git a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_trajectory_history.cpp b/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_trajectory_history.cpp deleted file mode 100644 index 16cc3f86d..000000000 --- a/smacc2_client_library/cl_moveit2z/src/cl_moveit2z/components/cp_trajectory_history.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#include - -namespace cl_moveit2z -{ -bool CpTrajectoryHistory::getLastTrajectory( - int backIndex, moveit_msgs::msg::RobotTrajectory & trajectory) -{ - if (trajectoryHistory_.size() == 0) - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << getName() << "] requested index: " << backIndex - << ", history size: " << trajectoryHistory_.size()); - return false; - } - - if (backIndex < 0) - - { - backIndex = 0; - } - else if ((size_t)backIndex >= this->trajectoryHistory_.size()) - { - RCLCPP_WARN_STREAM( - getLogger(), "[" << getName() << "] requested index: " << backIndex - << ", history size: " << trajectoryHistory_.size()); - return false; - } - - trajectory = this->trajectoryHistory_[this->trajectoryHistory_.size() - 1 - backIndex].trajectory; - return true; -} - -bool CpTrajectoryHistory::getLastTrajectory(moveit_msgs::msg::RobotTrajectory & trajectory) -{ - return getLastTrajectory(-1, trajectory); -} - -void CpTrajectoryHistory::pushTrajectory( - std::string name, const moveit_msgs::msg::RobotTrajectory & trajectory, - moveit_msgs::msg::MoveItErrorCodes result) -{ - RCLCPP_INFO_STREAM( - getLogger(), "[" << getName() << "] adding a new trajectory to the history ( " - << trajectory.joint_trajectory.points.size() << " poses)"); - - TrajectoryHistoryEntry entry; - this->trajectoryHistory_.push_back(entry); - - auto & last = this->trajectoryHistory_.back(); - last.trajectory = trajectory; - last.result = result; - last.name = name; -} - -} // namespace cl_moveit2z diff --git a/smacc2_dev_tools/claude/Prompts.md b/smacc2_dev_tools/claude/Prompts.md index d28ac54a9..c8151a547 100644 --- a/smacc2_dev_tools/claude/Prompts.md +++ b/smacc2_dev_tools/claude/Prompts.md @@ -66,3 +66,21 @@ StCircularPivotMotion  look in the log files (~/.ros/log) for the most recent state machine run. Analyse the move_group, state machine, and other logs and creat a plan to fix the error messages that are occuring in StMoveCartesianRelative2 + + + Think hard and make a plan, but don't code yet, to refactor the cl_moveit2z package so that its style follows a pure component based architecture. + +Organize the phases of the so that each phase can be successfully compiled and tested at runtime using the sm_panda_cl_moveit2z_cb_inventory package. + +CRITICAL: Do not modify the sm_panda_cl_moveit2z_cb_inventory package for any reason. + +All testing will be performed by me (the human). Let me know when to conduct a test and Ill let you know the results. + + +Ultrathink and make a plan, but don't code yet, to refactor the cl_moveit2z package so that all of the client behaviors are header-only. + +Organize the phases of the plan so that each phase can be successfully compiled and tested at runtime using the sm_panda_cl_moveit2z_cb_inventory package. + +CRITICAL: Do not modify the sm_panda_cl_moveit2z_cb_inventory package for any reason. + +All testing will be performed by me (the human). Let me know when to conduct a test and Ill let you know the results. diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/sm_panda_cl_moveit2z_cb_inventory.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/sm_panda_cl_moveit2z_cb_inventory.hpp index b168d61f5..27b9e3dd7 100644 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/sm_panda_cl_moveit2z_cb_inventory.hpp +++ b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/sm_panda_cl_moveit2z_cb_inventory.hpp @@ -60,26 +60,16 @@ struct StPause10; struct StPause11; struct StPause12; struct StPause13; -struct StPause14; -struct StPause15; -struct StPause16; struct StMoveJoints1; struct StMoveJoints2; struct StMoveJoints3; struct StMoveJoints4; struct StMoveJoints5; struct StMoveEndEffector; -struct StMoveCartesianRelative; struct StMoveCartesianRelative2; -struct StCircularPivotMotion; -struct StAttachObject; -struct StDetachObject; struct StEndEffectorRotate; -struct StExecuteLastTrajectory; struct StMoveKnownState1; struct StMoveKnownState2; -struct StMoveKnownState3; -struct StPouringMotion; struct StUndoLastTrajectory; //-------------------------------------------------------------------- @@ -113,26 +103,14 @@ struct SmPandaClMoveit2zCbInventory : public smacc2::SmaccStateMachineBase -#include - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StAttachObject : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPause11, SUCCESS>, - Transition, StPause11, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal("virtualBox"); - configure_orthogonal(); - } - - void runtimeConfigure() - { - RCLCPP_INFO(getLogger(), "Entering StAttachObject"); - cl_moveit2z::ClMoveit2z* moveGroupClient; - requiresClient(moveGroupClient); - } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp deleted file mode 100644 index 1c91cfa63..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StCircularPivotMotion : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPause13, SUCCESS>, - - Transition, StPause13, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StCircularPivotMotion"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_detach_object.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_detach_object.hpp deleted file mode 100644 index eac8c5ce9..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_detach_object.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StDetachObject : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPause12, SUCCESS>, - Transition, StPause12, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StDetachObject"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp deleted file mode 100644 index 47b5efa46..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StExecuteLastTrajectory : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StMoveLastTrajectoryInitialState, SUCCESS>, - - Transition, StUndoLastTrajectory, PREVIOUS>, - Transition, StMoveLastTrajectoryInitialState, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StExecuteLastTrajectory"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp deleted file mode 100644 index 8ef854049..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StMoveCartesianRelative : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - - Transition, StPause7, SUCCESS>, - // Transition, StAttachObject, SUCCESS> , - // Transition, StMoveCartesianRelative2, ABORT>, - - Transition, StPause7, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - geometry_msgs::msg::Vector3 offset; - offset.x = -0.01; - configure_orthogonal(offset); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveCartesianRelative"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_known_state_3.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_known_state_3.hpp deleted file mode 100644 index 97d85a733..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_known_state_3.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StMoveKnownState3 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPouringMotion, SUCCESS>, - Transition, StPouringMotion, ABORT>, - - Transition, StExecuteLastTrajectory, PREVIOUS>, - Transition, StPouringMotion, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - std::string pkg = "sm_panda_cl_moveit2z_cb_inventory"; - std::string filepath = "config/move_group_client/known_states/control_authority_posture.yaml"; - - configure_orthogonal(pkg, filepath); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveKnownState"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp deleted file mode 100644 index 5f2f03ade..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StMoveLastTrajectoryInitialState : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StUndoLastTrajectory, SUCCESS>, - Transition, StUndoLastTrajectory, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveLastTrajectoryInitialState"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_named_target.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_named_target.hpp deleted file mode 100644 index a771863a8..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_move_named_target.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StMoveNamedTarget : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - // Transition, StMoveCartesianRelative2, SUCCESS> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // geometry_msgs::msg::Vector3 position; - // position.x = -0.01; - // position.y = 0.0; - // position.z = 0.025; - // configure_orthogonal("tool0", "tool0", position); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveNamedTarget"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_12.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_12.hpp index 8d3142876..156d3bd08 100644 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_12.hpp +++ b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_12.hpp @@ -48,10 +48,10 @@ struct StPause12 : smacc2::SmaccState // TRANSITION TABLE typedef boost::mpl::list< - Transition, StCircularPivotMotion, SUCCESS>, - - Transition, StCircularPivotMotion, NEXT> - + Transition, StPause13, SUCCESS>, + + Transition, StPause13, NEXT> + > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_14.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_14.hpp deleted file mode 100644 index 1dfba6732..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_14.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::EvStateRequestFinish; -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_moveit2z; -using smacc2::client_behaviors::CbWaitTopicMessage; -using smacc2::client_behaviors::CbSleepFor; -using namespace std::chrono_literals; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StPause14 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPouringMotion, SUCCESS>, - - Transition, StPouringMotion, NEXT> - - - > reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal>("/joint_states"); - configure_orthogonal(15s); - configure_orthogonal(); - }; - - void runtimeConfigure() {} -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_15.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_15.hpp deleted file mode 100644 index 3a519c4fe..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_15.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::EvStateRequestFinish; -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_moveit2z; -using smacc2::client_behaviors::CbWaitTopicMessage; -using smacc2::client_behaviors::CbSleepFor; -using namespace std::chrono_literals; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StPause15 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPouringMotion, SUCCESS>, - - Transition, StPouringMotion, NEXT> - - - > reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal>("/joint_states"); - configure_orthogonal(15s); - configure_orthogonal(); - }; - - void runtimeConfigure() {} -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_16.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_16.hpp deleted file mode 100644 index 2c5286f88..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pause_16.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include -#include - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::EvStateRequestFinish; -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_moveit2z; -using smacc2::client_behaviors::CbWaitTopicMessage; -using smacc2::client_behaviors::CbSleepFor; -using namespace std::chrono_literals; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StPause16 : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPouringMotion, SUCCESS>, - - Transition, StPouringMotion, NEXT> - - - > reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - // configure_orthogonal>("/joint_states"); - configure_orthogonal(15s); - configure_orthogonal(); - }; - - void runtimeConfigure() {} -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pouring_motion.hpp b/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pouring_motion.hpp deleted file mode 100644 index 0b61f1af4..000000000 --- a/smacc2_sm_reference_library/sm_panda_cl_moveit2z_cb_inventory/include/sm_panda_cl_moveit2z_cb_inventory/states/st_pouring_motion.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2021 MyName/MyCompany Inc. -// Copyright 2021 RobosoftAI Inc. (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - *****************************************************************************************************************/ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "smacc2/smacc.hpp" - -namespace sm_panda_cl_moveit2z_cb_inventory -{ -// SMACC2 classes -using smacc2::Transition; -using smacc2::default_transition_tags::SUCCESS; -using namespace smacc2; -using namespace cl_keyboard; - -// STATE DECLARATION -struct StPouringMotion : smacc2::SmaccState -{ - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT TAGS - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef boost::mpl::list< - Transition, StPause9, SUCCESS>, - - Transition, StPause9, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - geometry_msgs::msg::Point relativePivotPoint; - relativePivotPoint.x = -0.01; - double deltaHeight = 0.05; - std::string tipLink = ""; // Auto-detect from MoveIt2 configuration - std::string globalFrame = "panda_link8"; // Standard Panda end effector link - - configure_orthogonal(relativePivotPoint, deltaHeight, tipLink, globalFrame); - configure_orthogonal(); - } - - void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StPouringMotion"); } - - void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } - - void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } -}; -} // namespace sm_panda_cl_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_simple_action_client/README.md b/smacc2_sm_reference_library/sm_simple_action_client/README.md index 216327415..389eed021 100644 --- a/smacc2_sm_reference_library/sm_simple_action_client/README.md +++ b/smacc2_sm_reference_library/sm_simple_action_client/README.md @@ -26,7 +26,8 @@ source ~/workspace/humble_ws/install/setup.sh And then run the launch file... ``` -ros2 launch simple_action_client_example simple_action_client_example.launch +ros2 launch sm_simple_action_client sm_simple_action_client.launch + ```

Viewer Instructions