AIRO-1211: Pick-and-Place Updates (#319)

* AIRO-1211: P&P initial cleanup

* RealSim fixes, ROS script updates

* Update changelog

* Pre-commit fixes

* PR feedback, demo fixes

* Pre-commit fixes

* Tutorial updates

* Updated scripts to run 0.6.0

Co-authored-by: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com>

Author: at669

CHANGELOG.md

@@ -10,6 +10,8 @@ Fixed the ROS-Unity Integration tutorial `robo_demo.launch` to be up-to-date wit
 
 Add the [Close Stale Issues](https://github.com/marketplace/actions/close-stale-issues) action
 
+Updated Pick-and-Place scripts for style conformity, updated custom msg formats and made according script and tutorial changes.
+
 ### Upgrade Notes
 
 ### Known Issues

tutorials/pick_and_place/0_ros_setup.md

@@ -39,7 +39,7 @@ git clone --recurse-submodules https://github.com/Unity-Technologies/Unity-Robot
 1. Start the newly built Docker container:
 
     ```docker
-    docker run -it --rm -p 10000:10000 -p 5005:5005 unity-robotics:pick-and-place /bin/bash
+    docker run -it --rm -p 10000:10000 unity-robotics:pick-and-place /bin/bash
     ```
 
     When this is complete, it will print: `Successfully tagged unity-robotics:pick-and-place`. This console should open into a bash shell at the ROS workspace root, e.g. `root@8d88ed579657:/catkin_ws#`.

tutorials/pick_and_place/2_ros_tcp.md

@@ -47,8 +47,6 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n
 
    In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory.
 
-   In the ROS Message Browser window, type in `Scripts/RosMessages` in the text field of Build message path.
-
    ![](img/2_browser.png)
 
    > Note: If any of these ROS directories appear to be empty, you can run the command `git submodule update --init --recursive` to download the packages via Git submodules.
@@ -87,33 +85,30 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n
 
    ```csharp
    public void Publish()
-   {
-      NiryoMoveitJointsMsg sourceDestinationMessage = new NiryoMoveitJointsMsg();
-
-      sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
-      sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
-      sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
-      sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
-      sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
-      sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
-
-      // Pick Pose
-      sourceDestinationMessage.pick_pose = new PoseMsg
-      {
-         position = target.transform.position.To<FLU>(),
-         // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
-         orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To<FLU>()
-      };
-
-      // Place Pose
-      sourceDestinationMessage.place_pose = new PoseMsg
-      {
-         position = targetPlacement.transform.position.To<FLU>(),
-         orientation = pickOrientation.To<FLU>()
-      };
-
-      // Finally send the message to server_endpoint.py running in ROS
-      ros.Send(topicName, sourceDestinationMessage);
+    {
+        var sourceDestinationMessage = new NiryoMoveitJointsMsg();
+
+        for (var i = 0; i < k_NumRobotJoints; i++)
+        {
+            sourceDestinationMessage.joints[i] = m_JointArticulationBodies[i].GetPosition();
+        }
+
+        // Pick Pose
+        sourceDestinationMessage.pick_pose = new PoseMsg
+        {
+            position = m_Target.transform.position.To<FLU>(),
+            orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To<FLU>()
+        };
+
+        // Place Pose
+        sourceDestinationMessage.place_pose = new PoseMsg
+        {
+            position = m_TargetPlacement.transform.position.To<FLU>(),
+            orientation = m_PickOrientation.To<FLU>()
+        };
+
+        // Finally send the message to server_endpoint.py running in ROS
+        m_Ros.Send(m_TopicName, sourceDestinationMessage);
    }
    ```
 

tutorials/pick_and_place/3_pick_and_place.md

@@ -31,30 +31,31 @@ Steps covered in this tutorial includes invoking a motion planning service in RO
     ```csharp
     public void PublishJoints()
     {
-        MoverServiceRequest request = new MoverServiceRequest();
+        var request = new MoverServiceRequest();
         request.joints_input = CurrentJointConfig();
 
         // Pick Pose
         request.pick_pose = new PoseMsg
         {
-            position = (target.transform.position + pickPoseOffset).To<FLU>(),
+            position = (m_Target.transform.position + m_PickPoseOffset).To<FLU>(),
+
             // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
-            orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To<FLU>()
+            orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To<FLU>()
         };
 
         // Place Pose
         request.place_pose = new PoseMsg
         {
-            position = (targetPlacement.transform.position + pickPoseOffset).To<FLU>(),
-            orientation = pickOrientation.To<FLU>()
+            position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To<FLU>(),
+            orientation = m_PickOrientation.To<FLU>()
         };
 
-        ros.SendServiceMessage<MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
+        m_Ros.SendServiceMessage<MoverServiceResponse>(m_RosServiceName, request, TrajectoryResponse);
     }
 
     void TrajectoryResponse(MoverServiceResponse response)
     {
-        if (response.trajectories != null)
+        if (response.trajectories.Length > 0)
         {
             Debug.Log("Trajectory returned.");
             StartCoroutine(ExecuteTrajectories(response));
@@ -137,12 +138,16 @@ def plan_trajectory(move_group, destination_pose, start_joint_angles):
     move_group.set_start_state(moveit_robot_state)
 
     move_group.set_pose_target(destination_pose)
-    plan = move_group.go(wait=True)
+    plan = move_group.plan()
 
     if not plan:
-        print("RAISE NO PLAN ERROR")
+        exception_str = """
+            Trajectory could not be planned for a destination of {} with starting joint angles {}.
+            Please make sure target and destination are reachable by the robot.
+        """.format(destination_pose, destination_pose)
+        raise Exception(exception_str)
 
-    return move_group.plan()
+    return planCompat(plan)
 ```
 
 > This creates a set of planned trajectories, iterating through a pre-grasp, grasp, pick up, and place set of poses. Finally, this set of trajectories is sent back to Unity.

tutorials/pick_and_place/PickAndPlaceProject/.gitignore

@@ -16,6 +16,7 @@
 # Ignore scripts that are expected to get copied in at some point
 /**/SourceDestinationPublisher.*
 /**/TrajectoryPlanner.*
+/**/RealSimPickAndPlace.*
 
 
 # MemoryCaptures can get excessive in size.