Performance Improvements

.devcontainer/devcontainer.json

@@ -10,13 +10,14 @@
 	"runArgs": [
 		// THIS NETWORK NEEDS TO EXIST. CREATE IT WITH: docker network create --subnet=192.168.56.0/24 ursim_net 
 		// This corresponds to the docker compose used in launching other services.
-		"--network=docker_ursim_net",
+		"--network=host",
 		"--cap-add=SYS_PTRACE",
 		"--security-opt=seccomp:unconfined",
 		"--security-opt=apparmor:unconfined",
 		"--volume=/tmp/.X11-unix:/tmp/.X11-unix",
 		// "--volume=/mnt/wslg:/mnt/wslg",
-		"--ipc=host"
+		"--ipc=host",
+		"--pid=host"
 	],
 	"containerEnv": {
 		"DISPLAY": "${localEnv:DISPLAY}", // Needed for GUI try ":0" for windows

docker/erobs-common-img/README.md

@@ -10,14 +10,15 @@ Several containers need to be running for our application. Once running, clients
 **TODO (ChandimaFernando)**: Change the repo link to nsls-II repo, and add CI/CD for build on version tag.
 
 ```bash
-export ROBOT_IP=192.168.0.100
+export ROBOT_IP=192.168.1.100
 export UR_TYPE="ur3e"
 export LAUNCH_RVIZ="false"
 export DESCRIPTION_PKG="ur3e_hande_robot_description"
 export DESCRIPTION_FILE="ur_with_camera_hande.xacro"
 export CONFIG_PKG="ur3e_hande_moveit_config"
 export CONFIG_FILE="ur.srdf"
 export ROS_DISTRO=humble
+export ROS_DOMAIN_ID=10
 export GHCR_POINTER=ghcr.io/chandimafernando/erobs-common-img:latest
 ```
 
@@ -29,6 +30,7 @@ podman run -it --rm --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
     --env DESCRIPTION_PKG=$DESCRIPTION_PKG \
     --env DESCRIPTION_FILE=$DESCRIPTION_FILE \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} description_package:=${DESCRIPTION_PKG} description_file:=${DESCRIPTION_FILE} launch_rviz:=${LAUNCH_RVIZ} tool_voltage:=24"
 ```
@@ -38,6 +40,7 @@ podman run -it --rm --network host --ipc=host --pid=host \
 podman run -it --rm --network host --ipc=host --pid=host \
     --env ROBOT_IP=$ROBOT_IP \
     --env ROS_DISTRO=$ROS_DISTRO  \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /bin/bash -c ". /root/ws/install/setup.sh && \
     . /opt/ros/${ROS_DISTRO}/setup.sh && \
@@ -56,6 +59,7 @@ The gripper_service has two convenience scripts, `gripper_open.cpp` and `gripper
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && \
     . /root/ws/install/setup.sh && \
@@ -73,6 +77,7 @@ podman run -it --rm --network host --ipc=host --pid=host \
     --env DESCRIPTION_FILE=$DESCRIPTION_FILE \
     --env CONFIG_PKG=$CONFIG_PKG \
     --env CONFIG_FILE=$CONFIG_FILE \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${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}"
 ```
@@ -81,6 +86,7 @@ podman run -it --rm --network host --ipc=host --pid=host \
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${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"
 ```
@@ -89,6 +95,7 @@ podman run -it --network host --ipc=host --pid=host \
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /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"
 ```
@@ -97,6 +104,7 @@ podman run -it --network host --ipc=host --pid=host \
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 action send_goal pdf_beamtime_action_server pdf_beamtime_interfaces/action/PickPlaceControlMsg '{pickup_approach: [1.571, -1.571, 0.0, -1.571, 0.0, 0.0]}'"
 ```
@@ -105,6 +113,7 @@ podman run -it --network host --ipc=host --pid=host \
 ```bash
 podman run -it --network host --ipc=host --pid=host \
     --env ROS_DISTRO=$ROS_DISTRO \
+    --env ROS_DOMAIN_ID=$ROS_DOMAIN_ID \
     ${GHCR_POINTER} \
     /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 action send_goal pdf_beamtime_action_server pdf_beamtime_interfaces/action/PickPlaceControlMsg {pickup_approach: [1.571, -1.571, 0.0, -1.571, 0.0, 0.0,]} --cancel"
 ```

src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp

@@ -12,6 +12,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/
 #include <string>
 #include <memory>
 #include <vector>
+#include <chrono>
 #include <utility>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>

src/pdf_beamtime/src/inner_state_machine.cpp

@@ -65,6 +65,7 @@ moveit::core::MoveItErrorCode InnerStateMachine::move_robot_cartesian(
   switch (internal_state_enum_) {
     case Internal_State::RESTING:
     case Internal_State::CLEANUP: {
+        mgi.setStartStateToCurrentState();
         // Create a plan to that target pose
         auto const [planing_success, plan] = [&mgi, target_pose] {
             moveit::planning_interface::MoveGroupInterface::Plan plan;

src/pdf_beamtime/src/tf_utilities.cpp

@@ -2,6 +2,7 @@
 BSD 3 Clause License. See LICENSE.txt for details.*/
 #include <pdf_beamtime/tf_utilities.hpp>
 
+using namespace std::chrono_literals;
 
 TFUtilities::TFUtilities(const rclcpp::Node::SharedPtr node)
 : node_(node), tf_util_logger_(node_->get_logger().get_child("tf_util"))
@@ -39,15 +40,9 @@ geometry_msgs::msg::TransformStamped TFUtilities::get_sample_pose(int sample_id)
   std::string sample_frame = std::to_string(sample_id);
   std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" +
     pre_pickup_approach_point_frame_suffix_;
-  while (rclcpp::ok()) {
-    try {
-      sample_pose =
-        tf_buffer_->lookupTransform(world_frame, sample_frame, tf2::TimePointZero);
 
-      break;
-    } catch (tf2::TransformException & ex) {
-    }
-  }
+  sample_pose =
+    tf_buffer_->lookupTransform(world_frame, sample_frame, tf2::TimePointZero, 10s);
 
   return sample_pose;
 }
@@ -59,17 +54,11 @@ geometry_msgs::msg::TransformStamped TFUtilities::get_sample_pre_pickup_pose(int
   std::string sample_frame = std::to_string(sample_id);
   std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" +
     pre_pickup_approach_point_frame_suffix_;
-  while (rclcpp::ok()) {
-    try {
-      pre_pickup_pose =
-        tf_buffer_->lookupTransform(
-        world_frame, pre_pickup_approach_point_frame,
-        tf2::TimePointZero);
-
-      break;
-    } catch (tf2::TransformException & ex) {
-    }
-  }
+
+  pre_pickup_pose =
+    tf_buffer_->lookupTransform(
+    world_frame, pre_pickup_approach_point_frame,
+    tf2::TimePointZero, 10s);
 
   return pre_pickup_pose;
 }
@@ -88,22 +77,15 @@ std::pair<double, double> TFUtilities::get_wrist_elbow_alignment(
   double wrist_2_roll, wrist_2_pitch, wrist_2_yaw;
   double sample_roll, sample_pitch, sample_yaw;
 
-  while (rclcpp::ok()) {
-    try {
-      transform_world_to_wrist_2 =
-        tf_buffer_->lookupTransform(world_frame, wrist_2_frame, tf2::TimePointZero);
-
-      tf2::fromMsg(transform_world_to_wrist_2.transform.rotation, tf2_wrist_2_quaternion);
-      tf2::fromMsg(transform_world_to_sample.transform.rotation, tf2_sample_quaternion);
+  transform_world_to_wrist_2 =
+    tf_buffer_->lookupTransform(world_frame, wrist_2_frame, tf2::TimePointZero, 10s);
 
-      // // Convert tf2::Quaternion to RPY
-      tf2::Matrix3x3(tf2_wrist_2_quaternion).getRPY(wrist_2_roll, wrist_2_pitch, wrist_2_yaw);
-      tf2::Matrix3x3(tf2_sample_quaternion).getRPY(sample_roll, sample_pitch, sample_yaw);
+  tf2::fromMsg(transform_world_to_wrist_2.transform.rotation, tf2_wrist_2_quaternion);
+  tf2::fromMsg(transform_world_to_sample.transform.rotation, tf2_sample_quaternion);
 
-      break;
-    } catch (tf2::TransformException & ex) {
-    }
-  }
+  // // Convert tf2::Quaternion to RPY
+  tf2::Matrix3x3(tf2_wrist_2_quaternion).getRPY(wrist_2_roll, wrist_2_pitch, wrist_2_yaw);
+  tf2::Matrix3x3(tf2_sample_quaternion).getRPY(sample_roll, sample_pitch, sample_yaw);
 
   // Get current joint values
   std::vector<double> joint_group_positions;
@@ -128,33 +110,25 @@ std::vector<geometry_msgs::msg::Pose> TFUtilities::get_pickup_action_z_adj(
 
   double z_dist_to_pickup_approach = 0.0;
 
-  while (rclcpp::ok()) {
-    try {
-      transform_world_to_grasping_point = tf_buffer_->lookupTransform(
-        world_frame,
-        grasping_point_on_gripper_frame,
-        tf2::TimePointZero);
-
-      z_dist_to_pickup_approach =
-        transform_world_to_sample.transform.translation.z -
-        (transform_world_to_grasping_point.transform.translation.z);
-
-      break;
-    } catch (tf2::TransformException & ex) {
-      RCLCPP_ERROR(
-        tf_util_logger_, "Could not transform %s to %s: %s",
-        world_frame.c_str(), grasping_point_on_gripper_frame.c_str(),
-        ex.what());
-    }
-  }
+  try {
+    transform_world_to_grasping_point = tf_buffer_->lookupTransform(
+      world_frame,
+      grasping_point_on_gripper_frame,
+      tf2::TimePointZero, 10s);
 
-  geometry_msgs::msg::Pose target_pose = mgi.getCurrentPose().pose;
-  // Move 2 cm down in the z direction
-  target_pose.position.z += z_dist_to_pickup_approach;
-  target_pose.position.z += -0.02;
+    z_dist_to_pickup_approach =
+      transform_world_to_sample.transform.translation.z -
+      (transform_world_to_grasping_point.transform.translation.z);
 
-  waypoints.push_back(target_pose);
+    geometry_msgs::msg::Pose target_pose = mgi.getCurrentPose().pose;
+    // Move 2 cm down in the z direction
+    target_pose.position.z += z_dist_to_pickup_approach;
+    target_pose.position.z += -0.02;
 
+    waypoints.push_back(target_pose);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(tf_util_logger_, e.what());
+  }
   return waypoints;
 }
 
@@ -169,28 +143,17 @@ std::vector<geometry_msgs::msg::Pose> TFUtilities::get_pickup_action_pre_pickup(
 
   double x_dist_to_pickup_approach = 0.0, y_dist_to_pickup_approach = 0.0;
 
-  while (rclcpp::ok()) {
-    try {
-      transform_world_to_grasping_point = tf_buffer_->lookupTransform(
-        world_frame,
-        grasping_point_on_gripper_frame,
-        tf2::TimePointZero);
-
-      x_dist_to_pickup_approach =
-        transform_world_to_pickup_approach_point.transform.translation.x -
-        transform_world_to_grasping_point.transform.translation.x;
-      y_dist_to_pickup_approach =
-        transform_world_to_pickup_approach_point.transform.translation.y -
-        transform_world_to_grasping_point.transform.translation.y;
-
-      break;
-    } catch (tf2::TransformException & ex) {
-      RCLCPP_ERROR(
-        tf_util_logger_, "Could not transform %s to %s: %s",
-        world_frame.c_str(),
-        grasping_point_on_gripper_frame.c_str(), ex.what());
-    }
-  }
+  transform_world_to_grasping_point = tf_buffer_->lookupTransform(
+    world_frame,
+    grasping_point_on_gripper_frame,
+    tf2::TimePointZero, 10s);
+
+  x_dist_to_pickup_approach =
+    transform_world_to_pickup_approach_point.transform.translation.x -
+    transform_world_to_grasping_point.transform.translation.x;
+  y_dist_to_pickup_approach =
+    transform_world_to_pickup_approach_point.transform.translation.y -
+    transform_world_to_grasping_point.transform.translation.y;
 
   geometry_msgs::msg::Pose target_pose = mgi.getCurrentPose().pose;
   target_pose.position.x += x_dist_to_pickup_approach;

src/ros2.repos

@@ -12,3 +12,7 @@ repositories:
     type: git
     url: https://github.com/PickNikRobotics/serial-release.git
     version: release/humble/serial
+  ur_description:
+    type: git
+    url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git
+    version: 21d5bfc277cffc7eb97c6a515deb8fc2001cc871