Humble fiducial pose integration into pdf_beamtime (#42)

* feat: wip tf_server

* feat: half-baked derived action server

* feat: add gripper open and close

* feat: add place

* feat: add gripper client calls

* fix: working gripper+place

* fix: correct coords in launch files

* fix: working return

* fix: lint errors

* feat: add joint return coords

* fix: corrected joint poses

* feat: separate fsm

* fix: add flag to force pickup before return

* fix: add goal abort when return

* fix: change goal accept condt.

docker/erobs-common-img/README.md

@@ -52,6 +52,16 @@ podman run -it --rm --network host --ipc=host --pid=host \
 `tool_communication.py` should not be called when `ur_control.launch.py` is called, but separately when the gripper is initiated.  The 5s delay allows socat creation before running the gripper service. 
 The gripper_service has two convenience scripts, `gripper_open.cpp` and `gripper_close.cpp` that will start up client nodes to send a request to the gripper service then promptly shutdown the client nodes.
 
+# Launch aruco_pose 
+```bash
+podman run -it --network host --ipc=host --pid=host \
+    --env ROS_DISTRO=$ROS_DISTRO \
+    ${GHCR_POINTER} \
+    /bin/sh -c "printenv && \
+    . /root/ws/install/setup.sh && \
+    . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    ros2 launch aruco_pose aruco_pose.launch.py"
+```
 
 # Launch move_group
 ```bash
@@ -66,23 +76,21 @@ podman run -it --rm --network host --ipc=host --pid=host \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=${UR_TYPE} launch_rviz:=${LAUNCH_RVIZ} description_package:=${DESCRIPTION_PKG}  launch_servo:=false description_file:=${DESCRIPTION_FILE} moveit_config_package:=${CONFIG_PKG} moveit_config_file:=${CONFIG_FILE}"
 ```
-# Launch aruco_pose 
+
+# Launch the pdf_beamtime_server
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
     ${GHCR_POINTER} \
-    /bin/sh -c "printenv && \
-    . /root/ws/install/setup.sh && \
-    . /opt/ros/${ROS_DISTRO}/setup.sh && \
-    ros2 launch aruco_pose aruco_pose.launch.py"
+    /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch pdf_beamtime pdf_beamtime.launch.py"
 ```
 
-# Launch the pdf_beamtime_server
+# Launch the pdf_beamtime_fidpose_server
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
     ${GHCR_POINTER} \
-    /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch pdf_beamtime pdf_beamtime.launch.py"
+    /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch pdf_beamtime pdf_beamtime_fidpose_server.launch.py"
 ```
 
 # Test base rotation for Emergency stop

src/pdf_beamtime/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(pdf_beamtime_interfaces REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(geometry_msgs)
 
 set(dependencies
   moveit_ros_planning_interface
@@ -21,15 +22,26 @@ set(dependencies
   rclcpp_action
   rclcpp_components
   yaml-cpp
+  geometry_msgs
 )
 
 include_directories(include)  # This makes sure the libraries are visible
 
-add_executable(pdf_beamtime_server src/pdf_beamtime_server.cpp src/inner_state_machine.cpp)
+add_executable(pdf_beamtime_server src/beamtime_entrypoint.cpp src/pdf_beamtime_server.cpp src/inner_state_machine.cpp)
 ament_target_dependencies(pdf_beamtime_server ${dependencies})
 
+add_executable(pdf_beamtime_fidpose_server
+    src/beamtime_fidpose_entrypoint.cpp
+    src/pdf_beamtime_fidpose_server.cpp
+    src/pdf_beamtime_server.cpp
+    src/inner_state_machine.cpp
+    src/tf_utilities.cpp
+)
+ament_target_dependencies(pdf_beamtime_fidpose_server ${dependencies})
+
 install(TARGETS
   pdf_beamtime_server
+  pdf_beamtime_fidpose_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

src/pdf_beamtime/config/joint_poses.yaml

@@ -1,4 +1,5 @@
 pdf_beamtime_server:
   ros__parameters:
     home_angles: [-0.11536, -1.783732, 0.38816, -1.75492, 0.11484, 3.14159] #For real robot
+    pickup_approach_angles: [5.1180035, -1.3447762, 2.08776285, -0.76043996, 3.48838958, 3.14159]
     gripper_present: true

src/pdf_beamtime/include/pdf_beamtime/inner_state_machine.hpp

@@ -4,21 +4,27 @@ BSD 3 Clause License. See LICENSE.txt for details.*/
 
 #include <moveit/move_group_interface/move_group_interface.h>
 
+#include <chrono>
 #include <future>
 #include <string>
 #include <map>
 #include <vector>
 #include <rclcpp/node.hpp>
 #include <pdf_beamtime/state_enum.hpp>
+#include <pdf_beamtime_interfaces/srv/gripper_control_msg.hpp>
 
 class InnerStateMachine
 {
 private:
   rclcpp::Node::SharedPtr node_;
+  rclcpp::Node::SharedPtr gripper_node_;
+
   Internal_State internal_state_enum_;
   /// @brief Holds the passed joint target
   std::vector<double> joint_goal_;
 
+  rclcpp::Client<pdf_beamtime_interfaces::srv::GripperControlMsg>::SharedPtr gripper_client_;
+
   std::vector<std::string> external_state_names_ =
   {"HOME", "PICKUP_APPROACH", "PICKUP", "GRASP_SUCCESS", "GRASP_FAILURE", "PICKUP_RETREAT",
     "PLACE_APPROACH", "PLACE", "RELEASE_SUCCESS", "RELEASE_FAILURE", "PLACE_RETREAT"};
@@ -27,7 +33,9 @@ class InnerStateMachine
   {"RESTING", "MOVING", "PAUSED", "ABORT", "HALT", "STOP", "CLEANUP"};
 
 public:
-  explicit InnerStateMachine(const rclcpp::Node::SharedPtr node);
+  explicit InnerStateMachine(
+    const rclcpp::Node::SharedPtr node,
+    const rclcpp::Node::SharedPtr gripper_node);
 
   /// @brief move the robot to the passed joint angles
   /// @param mgi move_group_interface_
@@ -36,6 +44,10 @@ class InnerStateMachine
     moveit::planning_interface::MoveGroupInterface & mgi,
     std::vector<double> joint_goal);
 
+  moveit::core::MoveItErrorCode move_robot_cartesian(
+    moveit::planning_interface::MoveGroupInterface & mgi,
+    std::vector<geometry_msgs::msg::Pose> target_pose);
+
   /// @brief state change if paused command was issues
   void pause(moveit::planning_interface::MoveGroupInterface & mgi);
   // State resume(moveit::planning_interface::MoveGroupInterface & mgi);
@@ -52,8 +64,7 @@ class InnerStateMachine
   void set_internal_state(Internal_State state);
   Internal_State get_internal_state();
 
-  /// @brief  Gripper object is not set at the pdf_beamtime level
-  /// @todo ChandimaFernando
+  /// @brief send client requests to open and close the gripper
   /// @return Moveit error code
   moveit::core::MoveItErrorCode open_gripper();
   moveit::core::MoveItErrorCode close_gripper();

src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp

@@ -0,0 +1,60 @@
+/*Copyright 2024 Brookhaven National Laboratory
+BSD 3 Clause License. See LICENSE.txt for details.*/
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <pdf_beamtime/pdf_beamtime_server.hpp>
+#include <pdf_beamtime/tf_utilities.hpp>
+#include <pdf_beamtime_interfaces/action/fid_pose_control_msg.hpp>
+
+/// @brief Create the obstacle environment and an simple action server for the robot to move
+class PdfBeamtimeFidPoseServer : public PdfBeamtimeServer
+{
+public:
+  using FidPoseControlMsg = pdf_beamtime_interfaces::action::FidPoseControlMsg;
+
+  explicit PdfBeamtimeFidPoseServer(
+    const std::string & move_group_name, const rclcpp::NodeOptions & options,
+    std::string action_name);
+
+private:
+  moveit::core::MoveItErrorCode run_fsm(
+    std::shared_ptr<const pdf_beamtime_interfaces::action::FidPoseControlMsg_Goal> goal);
+
+  moveit::core::MoveItErrorCode run_return_fsm(
+    std::shared_ptr<const pdf_beamtime_interfaces::action::FidPoseControlMsg_Goal> goal);
+
+  /// @brief Pointer to the inner state machine object
+  TFUtilities * tf_utilities_;
+  std::vector<double, std::allocator<double>> adjusted_pickup_;
+  std::vector<double, std::allocator<double>> adjusted_place_;
+
+  /// @brief holds the current goal to be used by return_sample() function
+  std::shared_ptr<const pdf_beamtime_interfaces::action::FidPoseControlMsg_Goal> fidpose_goal;
+
+  /// @brief records pickup approach state
+  std::vector<double, std::allocator<double>> pickup_approach_;
+
+  rclcpp_action::Server<FidPoseControlMsg>::SharedPtr fidpose_action_server_;
+
+  void fidpose_handle_accepted(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<FidPoseControlMsg>> goal_handle);
+  rclcpp_action::GoalResponse fidpose_handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const FidPoseControlMsg::Goal> goal);
+  rclcpp_action::CancelResponse fidpose_handle_cancel(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<FidPoseControlMsg>> goal_handle);
+
+  virtual void execute(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<FidPoseControlMsg>> goal_handle);
+
+  moveit::core::MoveItErrorCode return_sample();
+  void execute_cleanup();
+
+  bool pickup_pose_saved = false;
+  std::vector<double> pre_pickup_approach_joints_;
+  std::vector<double> pickup_joints_;
+};

src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_server.hpp

@@ -24,6 +24,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/
 #include <pdf_beamtime_interfaces/srv/box_obstacle_msg.hpp>
 #include <pdf_beamtime_interfaces/srv/cylinder_obstacle_msg.hpp>
 #include <pdf_beamtime_interfaces/srv/bluesky_interrupt_msg.hpp>
+#include <pdf_beamtime_interfaces/srv/gripper_control_msg.hpp>
 #include <pdf_beamtime/inner_state_machine.hpp>
 #include <pdf_beamtime/state_enum.hpp>
 
@@ -44,10 +45,26 @@ class PdfBeamtimeServer
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getInterruptNodeBaseInterface();
 
-private:
+protected:
   rclcpp::Node::SharedPtr node_;
+  /// @brief Pointer to the action server
+  rclcpp_action::Server<PickPlaceControlMsg>::SharedPtr action_server_;
+
+  virtual void handle_accepted(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
+  // Action server related callbacks
+  virtual void execute(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
+
+  /// @brief Performs the transitions for each State
+  virtual moveit::core::MoveItErrorCode run_fsm(
+    std::shared_ptr<const pdf_beamtime_interfaces::action::PickPlaceControlMsg_Goal> goal);
+
   rclcpp::Node::SharedPtr interrupt_node_;
 
+  // A separate node for the gripper
+  rclcpp::Node::SharedPtr gripper_node_;
+
   moveit::planning_interface::MoveGroupInterface move_group_interface_;
 
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
@@ -60,8 +77,6 @@ class PdfBeamtimeServer
   rclcpp::Service<DeleteObstacleMsg>::SharedPtr remove_obstacles_service_;
   rclcpp::Service<BlueskyInterruptMsg>::SharedPtr bluesky_interrupt_service_;
 
-  /// @brief Pointer to the action server
-  rclcpp_action::Server<PickPlaceControlMsg>::SharedPtr action_server_;
 
   /// @brief Pointer to the inner state machine object
   InnerStateMachine * inner_state_machine_;
@@ -99,20 +114,6 @@ class PdfBeamtimeServer
 /// @todo @ChandimaFernando Implement to see if gripper is attached
   bool gripper_present_ = false;
 
-  // Action server related callbacks
-  rclcpp_action::GoalResponse handle_goal(
-    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const PickPlaceControlMsg::Goal> goal);
-
-  rclcpp_action::CancelResponse handle_cancel(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
-
-  void handle_accepted(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
-
-  void execute(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
-
   /// @brief generates a vector of obstacles from a yaml file.
   /// @return a vector of CollisionObjects
   std::vector<moveit_msgs::msg::CollisionObject> create_env();
@@ -149,21 +150,27 @@ class PdfBeamtimeServer
   /// @brief Set the current state to the next state
   float get_action_completion_percentage();
 
-  /// @brief Performs the transitions for each State
-  moveit::core::MoveItErrorCode run_fsm(
-    std::shared_ptr<const pdf_beamtime_interfaces::action::PickPlaceControlMsg_Goal> goal);
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const PickPlaceControlMsg::Goal> goal);
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<PickPlaceControlMsg>> goal_handle);
+
+  // /// @brief Performs the transitions for each State
+  // moveit::core::MoveItErrorCode run_fsm(
+  //   std::shared_ptr<const pdf_beamtime_interfaces::action::PickPlaceControlMsg_Goal> goal);
 
   /// @brief Set the current state to HOME and move robot to home position
   bool reset_fsm();
 
   /// @brief Put back the sample
-  moveit::core::MoveItErrorCode return_sample();
+  virtual moveit::core::MoveItErrorCode return_sample();
 
   /// @brief Handles bluesky interrupt to PAUSE, STOP, ABORT, and HALT
   void handle_pause();
   void handle_stop();
   /// @brief returns the sample to where it was picked and ready robot to receive a new goal
-  void execute_cleanup();
+  virtual void execute_cleanup();
   void handle_abort();
   void handle_halt();
 

src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp

@@ -0,0 +1,52 @@
+/*Copyright 2024 Brookhaven National Laboratory
+BSD 3 Clause License. See LICENSE.txt for details.*/
+#pragma once
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <utility>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node.hpp>
+
+class TFUtilities
+{
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Logger tf_util_logger_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+
+  std::shared_ptr<rclcpp::SyncParametersClient> parameters_client_;
+
+  std::string world_frame, sample_frame, grasping_point_on_gripper_frame, wrist_2_frame,
+    pre_pickup_approach_point_frame;
+
+public:
+  explicit TFUtilities(const rclcpp::Node::SharedPtr node);
+
+  /// @brief converts degrees to radians
+  double degreesToRadians(double degrees);
+
+  std::pair<double, double> get_wrist_elbow_alignment(
+    moveit::planning_interface::MoveGroupInterface & mgi);
+
+  std::vector<geometry_msgs::msg::Pose> get_pickup_action_z_adj(
+    moveit::planning_interface::MoveGroupInterface & mgi);
+
+  std::vector<geometry_msgs::msg::Pose> get_pickup_action_pre_pickup(
+    moveit::planning_interface::MoveGroupInterface & mgi);
+
+  std::vector<geometry_msgs::msg::Pose> get_pickup_action_pickup(
+    moveit::planning_interface::MoveGroupInterface & mgi);
+};

src/pdf_beamtime/launch/pdf_beamtime_fidpose_server.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    """Launch the node obstacle_builder with a parameter file."""
+    action_cmd = Node(
+        package="pdf_beamtime",
+        executable="pdf_beamtime_fidpose_server",
+        parameters=[
+            PathJoinSubstitution([FindPackageShare("pdf_beamtime"), "config", "obstacles.yaml"]),
+            PathJoinSubstitution([FindPackageShare("pdf_beamtime"), "config", "joint_poses.yaml"]),
+        ],
+        output="screen",
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(action_cmd)
+
+    return ld