Release 3

CMakeLists.txt

@@ -1,29 +1,28 @@
 cmake_minimum_required(VERSION 3.8)
 project(pick_place_fsm)
 
-set(CMAKE_CXX_STANDARD 20)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(eddie_ros REQUIRED)   # ← only for .action headers
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/CaptureReference.srv"
+)
+
+install(PROGRAMS
+  src/pick_place.py
+  DESTINATION lib/${PROJECT_NAME}
+  RENAME pick_place
+)
 
-# ----------  CLIENT NODE  ----------
-add_executable(pick_place_fsm_mock
-  src/pick_place_fsm_mock.cpp)
-ament_target_dependencies(pick_place_fsm_mock
-  rclcpp rclcpp_action geometry_msgs eddie_ros)
-install(TARGETS pick_place_fsm_mock
-  DESTINATION lib/${PROJECT_NAME})
+install(PROGRAMS
+  include/fsm_pick_place.py
+  src/error_handling.py
+  DESTINATION lib/${PROJECT_NAME}
+)
 
-# ----------  MOCK SERVER NODE  ----------
-add_executable(mock_servers_only
-  src/mock_servers_only.cpp)
-ament_target_dependencies(mock_servers_only
-  rclcpp rclcpp_action eddie_ros)   # geometry_msgs pulled in by eddie_ros
-install(TARGETS mock_servers_only
-  DESTINATION lib/${PROJECT_NAME})
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
 ament_package()

README.md

@@ -0,0 +1,59 @@
+# Pick and Place FSM (Python)
+
+This package contains a Python pick-and-place FSM client that talks to the arm/gripper action servers and delegates arm motion to the spline planner.
+
+## FSM Overview
+
+![FSM diagram](images/fsm_diagram.png)
+
+States (see `include/pick_place_py.fsm`):
+- `S_IDLE` → waits for start
+- `S_POSE_DETECTION` → listens for `/object_pose`
+- `S_MOVE_ARM` → send arm motion (via spline planner action)
+- `S_CLOSE_GRIPPER` → close on object
+- `S_OPEN_GRIPPER` → release object
+- `S_FINISHED` → success path
+- `S_MOVE_ARM_HOME` → return home
+- `S_EXIT` → terminal state
+
+Key events: `E_START`, `E_PERCEPTION_POSE`, `E_PICK_MOVE_DONE`, `E_PLACE_MOVE_DONE`, `E_GRIPPER_CLOSE_DONE_OK`, `E_OPEN_DONE_OK`, `E_HOME_DONE_OK`, `E_ARM_MOVE_DONE_FAIL`, `E_HOME_DONE_FAIL`, `E_PERCEPTION_FAIL`, `E_GO_HOME`.
+
+## Runtime Topics & Actions
+- Subscribes: `/object_pose` (`geometry_msgs/PoseStamped`)
+- Actions (real or mock):
+  - `right_arm/arm_control` (`eddie_ros/ArmControl`)
+  - `right_arm/gripper_control` (`eddie_ros/GripperControl`)
+- Planner: `spline_plan` (`cartesian_planner/PlanSpline`) for generating/executing waypoints
+
+## Object Slip Detection
+- Reference capture
+After the gripper successfully finishes a close action, the pick-and-place FSM calls the service
+/gripper_slip/capture_reference.
+At that moment, the slip detector latches the current measured gripper opening derived from the gripper joint encoder in /joint_states.
+The detector converts the joint position of eddie_right_arm_robotiq_85_left_knuckle_joint
+into a percentage opening (0–100 %) and stores this single value as the reference position.
+
+- Motion check
+During subsequent motion, the slip detector continuously listens to /joint_states and updates the current measured gripper opening in percent.
+The detector compares each incoming measurement against the stored reference.
+If the relative drift between the current value and the reference exceeds a configurable threshold (drift_thresh_percent), the node flags a slip event and publishes True on /gripper_slip
+
+drift : |current − reference| / reference
+
+## Quick Start
+```
+colcon build
+source install/setup.bash
+# Eddie interface depending on simulation or real hardware
+
+ros2 run cartesian_planner spline_planner     # planner
+ros2 run pick_place_fsm pick_place            # FSM client
+ros2 run pick_place_fsm error_handling.py     # object slip detection
+```
+### Refer [Detail steps](how_to_run.md) on how to run with complete robot setup
+
+## Regenerating FSM Code(.fsm)
+Requires `coord-dsl`:
+```
+textx generate include/pick_place_py.fsm --target fsm_py -o include/fsm_pick_place.py
+```