From 62e436af51f6be945ba6851a46a80341f53968e0 Mon Sep 17 00:00:00 2001 From: at669 Date: Mon, 27 Sep 2021 11:55:46 -0600 Subject: [PATCH 1/8] AIRO-1211: P&P initial cleanup --- .../Packages/manifest.json | 3 +- .../ROS/src/niryo_moveit/launch/part_2.launch | 2 +- .../ROS/src/niryo_moveit/launch/part_3.launch | 2 +- .../ROS/src/niryo_moveit/launch/part_4.launch | 2 +- .../niryo_moveit/msg/NiryoMoveitJoints.msg | 7 +- .../niryo_moveit/scripts/server_endpoint.py | 28 --- .../pick_and_place/ROS/src/ros_tcp_endpoint | 2 +- .../Scripts/SourceDestinationPublisher.cs | 83 +++---- .../Scripts/TrajectoryPlanner.cs | 204 +++++++++--------- .../ros_docker/Dockerfile | 2 +- 10 files changed, 149 insertions(+), 186 deletions(-) delete mode 100755 tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index b5d47257..49e6c37c 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -3,7 +3,8 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.8", "com.unity.ide.vscode": "1.2.3", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.5.0", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#laurie/VisualizationPackage", + "com.unity.robotics.visualizations": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations#laurie/VisualizationPackage", "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.5.0", "com.unity.test-framework": "1.1.24", "com.unity.textmeshpro": "3.0.6", diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_2.launch b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_2.launch index 16a7600c..4c28027d 100644 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_2.launch +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_2.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_3.launch b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_3.launch index a969fdf4..b3b40711 100644 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_3.launch +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_3.launch @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_4.launch b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_4.launch index aa3c5a3f..432407c3 100644 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_4.launch +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_4.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/msg/NiryoMoveitJoints.msg b/tutorials/pick_and_place/ROS/src/niryo_moveit/msg/NiryoMoveitJoints.msg index f463dd66..a673bb8a 100644 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/msg/NiryoMoveitJoints.msg +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/msg/NiryoMoveitJoints.msg @@ -1,8 +1,3 @@ -float64 joint_00 -float64 joint_01 -float64 joint_02 -float64 joint_03 -float64 joint_04 -float64 joint_05 +float64[6] joints geometry_msgs/Pose pick_pose geometry_msgs/Pose place_pose diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py deleted file mode 100755 index 9d9c65ac..00000000 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python - -import rospy - -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService -from niryo_moveit.msg import NiryoMoveitJoints, NiryoTrajectory -from niryo_moveit.srv import MoverService, MoverServiceRequest - -from niryo_one_msgs.msg import RobotMoveActionGoal - -def main(): - ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') - tcp_server = TcpServer(ros_node_name) - rospy.init_node(ros_node_name, anonymous=True) - - # Start the Server Endpoint with a ROS communication objects dictionary for routing messages - tcp_server.start({ - 'SourceDestination_input': RosPublisher('SourceDestination_input', NiryoMoveitJoints, queue_size=10), - 'NiryoTrajectory': RosSubscriber('NiryoTrajectory', NiryoTrajectory, tcp_server), - 'niryo_moveit': RosService('niryo_moveit', MoverService), - 'niryo_one/commander/robot_action/goal': RosSubscriber('niryo_one/commander/robot_action/goal', RobotMoveActionGoal, tcp_server), - 'sim_real_pnp': RosPublisher('sim_real_pnp', MoverServiceRequest) - }) - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint index f978d948..fce165aa 160000 --- a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint +++ b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint @@ -1 +1 @@ -Subproject commit f978d948c35e5c14542839b5f8c2e715c4b61a1d +Subproject commit fce165aa00057936df589274e242c11f2b90488c diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index df69b6ae..8798cefb 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -1,82 +1,83 @@ +using System; +using RosMessageTypes.Geometry; using RosMessageTypes.NiryoMoveit; -using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; using Unity.Robotics.UrdfImporter; -using RosMessageTypes.Geometry; +using UnityEngine; public class SourceDestinationPublisher : MonoBehaviour { - // ROS Connector - private ROSConnection ros; + const int k_NumRobotJoints = 6; // Variables required for ROS communication - public string topicName = "SourceDestination_input"; + [SerializeField] + string m_TopicName = "/niryo_joints"; - public GameObject niryoOne; - public GameObject target; - public GameObject targetPlacement; - - private int numRobotJoints = 6; - private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); + [SerializeField] + GameObject m_NiryoOne; + [SerializeField] + GameObject m_Target; + [SerializeField] + GameObject m_TargetPlacement; + readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0); // Robot Joints - private UrdfJointRevolute[] revoluteJoints; + UrdfJointRevolute[] m_JointArticulationBodies; + + // ROS Connector + ROSConnection m_Ros; - /// - /// - /// void Start() { // Get ROS connection static instance - ros = ROSConnection.instance; + m_Ros = ROSConnection.GetOrCreateInstance(); + m_Ros.RegisterPublisher(m_TopicName); - revoluteJoints = new UrdfJointRevolute[numRobotJoints]; - string shoulder_link = "world/base_link/shoulder_link"; - revoluteJoints[0] = niryoOne.transform.Find(shoulder_link).GetComponent(); + m_JointArticulationBodies = new UrdfJointRevolute[k_NumRobotJoints]; + var linkName = "world/base_link/shoulder_link"; + m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); - string arm_link = shoulder_link + "/arm_link"; - revoluteJoints[1] = niryoOne.transform.Find(arm_link).GetComponent(); + linkName += "/arm_link"; + m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); - string elbow_link = arm_link + "/elbow_link"; - revoluteJoints[2] = niryoOne.transform.Find(elbow_link).GetComponent(); + linkName += "/elbow_link"; + m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); - string forearm_link = elbow_link + "/forearm_link"; - revoluteJoints[3] = niryoOne.transform.Find(forearm_link).GetComponent(); + linkName += "/forearm_link"; + m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); - string wrist_link = forearm_link + "/wrist_link"; - revoluteJoints[4] = niryoOne.transform.Find(wrist_link).GetComponent(); + linkName += "/wrist_link"; + m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); - string hand_link = wrist_link + "/hand_link"; - revoluteJoints[5] = niryoOne.transform.Find(hand_link).GetComponent(); + linkName += "/hand_link"; + m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); } public void Publish() { - NiryoMoveitJointsMsg sourceDestinationMessage = new NiryoMoveitJointsMsg(); + var sourceDestinationMessage = new NiryoMoveitJointsMsg(); - sourceDestinationMessage.joint_00 = revoluteJoints[0].GetPosition() * Mathf.Rad2Deg; - sourceDestinationMessage.joint_01 = revoluteJoints[1].GetPosition() * Mathf.Rad2Deg; - sourceDestinationMessage.joint_02 = revoluteJoints[2].GetPosition() * Mathf.Rad2Deg; - sourceDestinationMessage.joint_03 = revoluteJoints[3].GetPosition() * Mathf.Rad2Deg; - sourceDestinationMessage.joint_04 = revoluteJoints[4].GetPosition() * Mathf.Rad2Deg; - sourceDestinationMessage.joint_05 = revoluteJoints[5].GetPosition() * Mathf.Rad2Deg; + for (var i = 0; i < k_NumRobotJoints; i++) + { + sourceDestinationMessage.joints[i] = m_JointArticulationBodies[i].GetPosition(); + } // Pick Pose sourceDestinationMessage.pick_pose = new PoseMsg { - position = target.transform.position.To(), - orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() + position = m_Target.transform.position.To(), + orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To() }; // Place Pose sourceDestinationMessage.place_pose = new PoseMsg { - position = targetPlacement.transform.position.To(), - orientation = pickOrientation.To() + position = m_TargetPlacement.transform.position.To(), + orientation = m_PickOrientation.To() }; // Finally send the message to server_endpoint.py running in ROS - ros.Send(topicName, sourceDestinationMessage); + m_Ros.Send(m_TopicName, sourceDestinationMessage); } } diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index 19aec70e..8c3679dd 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -1,77 +1,107 @@ +using System; using System.Collections; using System.Linq; +using RosMessageTypes.Geometry; using RosMessageTypes.NiryoMoveit; -using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; -using RosMessageTypes.Geometry; +using UnityEngine; public class TrajectoryPlanner : MonoBehaviour { - // ROS Connector - private ROSConnection ros; - - // Hardcoded variables - private int numRobotJoints = 6; - private readonly float jointAssignmentWait = 0.1f; - private readonly float poseAssignmentWait = 0.5f; - private readonly Vector3 pickPoseOffset = Vector3.up * 0.1f; - - // Assures that the gripper is always positioned above the target cube before grasping. - private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); + // Hardcoded variables + const int k_NumRobotJoints = 6; + const float k_JointAssignmentWait = 0.1f; + const float k_PoseAssignmentWait = 0.5f; // Variables required for ROS communication - public string rosServiceName = "niryo_moveit"; + [SerializeField] + string m_RosServiceName = "niryo_moveit"; + + [SerializeField] + GameObject m_NiryoOne; + [SerializeField] + GameObject m_Target; + [SerializeField] + GameObject m_TargetPlacement; - public GameObject niryoOne; - public GameObject target; - public GameObject targetPlacement; + // Assures that the gripper is always positioned above the target cube before grasping. + readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0); + readonly Vector3 m_PickPoseOffset = Vector3.up * 0.1f; // Articulation Bodies - private ArticulationBody[] jointArticulationBodies; - private ArticulationBody leftGripper; - private ArticulationBody rightGripper; + ArticulationBody[] m_JointArticulationBodies; + ArticulationBody m_LeftGripper; + ArticulationBody m_RightGripper; - private Transform gripperBase; - private Transform leftGripperGameObject; - private Transform rightGripperGameObject; + // ROS Connector + ROSConnection m_Ros; - private enum Poses + /// + /// Find all robot joints in Awake() and add them to the jointArticulationBodies array. + /// Find left and right finger joints and assign them to their respective articulation body objects. + /// + void Start() { - PreGrasp, - Grasp, - PickUp, - Place - }; + // Get ROS connection static instance + m_Ros = ROSConnection.GetOrCreateInstance(); + m_Ros.RegisterRosService(m_RosServiceName); + + m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints]; + var linkName = "world/base_link/shoulder_link"; + m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/arm_link"; + m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/elbow_link"; + m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/forearm_link"; + m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/wrist_link"; + m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/hand_link"; + m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + // Find left and right fingers + var rightGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; + var leftGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper"; + + m_RightGripper = m_NiryoOne.transform.Find(rightGripper).GetComponent(); + m_LeftGripper = m_NiryoOne.transform.Find(leftGripper).GetComponent(); + } /// /// Close the gripper /// - private void CloseGripper() + void CloseGripper() { - var leftDrive = leftGripper.xDrive; - var rightDrive = rightGripper.xDrive; + var leftDrive = m_LeftGripper.xDrive; + var rightDrive = m_RightGripper.xDrive; leftDrive.target = -0.01f; rightDrive.target = 0.01f; - leftGripper.xDrive = leftDrive; - rightGripper.xDrive = rightDrive; + m_LeftGripper.xDrive = leftDrive; + m_RightGripper.xDrive = rightDrive; } /// /// Open the gripper /// - private void OpenGripper() + void OpenGripper() { - var leftDrive = leftGripper.xDrive; - var rightDrive = rightGripper.xDrive; + var leftDrive = m_LeftGripper.xDrive; + var rightDrive = m_RightGripper.xDrive; leftDrive.target = 0.01f; rightDrive.target = -0.01f; - leftGripper.xDrive = leftDrive; - rightGripper.xDrive = rightDrive; + m_LeftGripper.xDrive = leftDrive; + m_RightGripper.xDrive = rightDrive; } /// @@ -80,14 +110,12 @@ private void OpenGripper() /// NiryoMoveitJoints NiryoMoveitJointsMsg CurrentJointConfig() { - NiryoMoveitJointsMsg joints = new NiryoMoveitJointsMsg(); + var joints = new NiryoMoveitJointsMsg(); - joints.joint_00 = jointArticulationBodies[0].jointPosition[0] * Mathf.Rad2Deg; - joints.joint_01 = jointArticulationBodies[1].jointPosition[0] * Mathf.Rad2Deg; - joints.joint_02 = jointArticulationBodies[2].jointPosition[0] * Mathf.Rad2Deg; - joints.joint_03 = jointArticulationBodies[3].jointPosition[0] * Mathf.Rad2Deg; - joints.joint_04 = jointArticulationBodies[4].jointPosition[0] * Mathf.Rad2Deg; - joints.joint_05 = jointArticulationBodies[5].jointPosition[0] * Mathf.Rad2Deg; + for (var i = 0; i < k_NumRobotJoints; i++) + { + joints.joints[i] = m_JointArticulationBodies[i].jointPosition[0]; + } return joints; } @@ -95,31 +123,31 @@ NiryoMoveitJointsMsg CurrentJointConfig() /// /// Create a new MoverServiceRequest with the current values of the robot's joint angles, /// the target cube's current position and rotation, and the targetPlacement position and rotation. - /// /// Call the MoverService using the ROSConnection and if a trajectory is successfully planned, /// execute the trajectories in a coroutine. /// 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(), + position = (m_Target.transform.position + m_PickPoseOffset).To(), + // 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() + orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To() }; // Place Pose request.place_pose = new PoseMsg { - position = (targetPlacement.transform.position + pickPoseOffset).To(), - orientation = pickOrientation.To() + position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To(), + orientation = m_PickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + m_Ros.SendServiceMessage(m_RosServiceName, request, TrajectoryResponse); } void TrajectoryResponse(MoverServiceResponse response) @@ -137,40 +165,38 @@ void TrajectoryResponse(MoverServiceResponse response) /// /// Execute the returned trajectories from the MoverService. - /// /// The expectation is that the MoverService will return four trajectory plans, - /// PreGrasp, Grasp, PickUp, and Place, + /// PreGrasp, Grasp, PickUp, and Place, /// where each plan is an array of robot poses. A robot pose is the joint angle values /// of the six robot joints. - /// /// Executing a single trajectory will iterate through every robot pose in the array while updating the /// joint values on the robot. - /// /// /// MoverServiceResponse received from niryo_moveit mover service running in ROS /// - private IEnumerator ExecuteTrajectories(MoverServiceResponse response) + IEnumerator ExecuteTrajectories(MoverServiceResponse response) { if (response.trajectories != null) { // For every trajectory plan returned - for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++) + for (var poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++) { // For every robot pose in trajectory plan - for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) + foreach (var t in response.trajectories[poseIndex].joint_trajectory.points) { - var jointPositions = response.trajectories[poseIndex].joint_trajectory.points[jointConfigIndex].positions; - float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); + var jointPositions = t.positions; + var result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); // Set the joint values for every joint - for (int joint = 0; joint < jointArticulationBodies.Length; joint++) + for (var joint = 0; joint < m_JointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = m_JointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; - jointArticulationBodies[joint].xDrive = joint1XDrive; + m_JointArticulationBodies[joint].xDrive = joint1XDrive; } + // Wait for robot to achieve pose for all joint assignments - yield return new WaitForSeconds(jointAssignmentWait); + yield return new WaitForSeconds(k_JointAssignmentWait); } // Close the gripper if completed executing the trajectory for the Grasp pose @@ -178,51 +204,19 @@ private IEnumerator ExecuteTrajectories(MoverServiceResponse response) CloseGripper(); // Wait for the robot to achieve the final pose from joint assignment - yield return new WaitForSeconds(poseAssignmentWait); + yield return new WaitForSeconds(k_PoseAssignmentWait); } + // All trajectories have been executed, open the gripper to place the target cube OpenGripper(); } } - /// - /// Find all robot joints in Awake() and add them to the jointArticulationBodies array. - /// Find left and right finger joints and assign them to their respective articulation body objects. - /// - void Start() + enum Poses { - // Get ROS connection static instance - ros = ROSConnection.instance; - - jointArticulationBodies = new ArticulationBody[numRobotJoints]; - string shoulder_link = "world/base_link/shoulder_link"; - jointArticulationBodies[0] = niryoOne.transform.Find(shoulder_link).GetComponent(); - - string arm_link = shoulder_link + "/arm_link"; - jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent(); - - string elbow_link = arm_link + "/elbow_link"; - jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent(); - - string forearm_link = elbow_link + "/forearm_link"; - jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent(); - - string wrist_link = forearm_link + "/wrist_link"; - jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent(); - - string hand_link = wrist_link + "/hand_link"; - jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent(); - - // Find left and right fingers - string right_gripper = hand_link + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; - string left_gripper = hand_link + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper"; - string gripper_base = hand_link + "/tool_link/gripper_base/Collisions/unnamed"; - - gripperBase = niryoOne.transform.Find(gripper_base); - leftGripperGameObject = niryoOne.transform.Find(left_gripper); - rightGripperGameObject = niryoOne.transform.Find(right_gripper); - - rightGripper = rightGripperGameObject.GetComponent(); - leftGripper = leftGripperGameObject.GetComponent(); + PreGrasp, + Grasp, + PickUp, + Place } } diff --git a/tutorials/ros_unity_integration/ros_docker/Dockerfile b/tutorials/ros_unity_integration/ros_docker/Dockerfile index f2ea2a15..3eeb644a 100644 --- a/tutorials/ros_unity_integration/ros_docker/Dockerfile +++ b/tutorials/ros_unity_integration/ros_docker/Dockerfile @@ -5,7 +5,7 @@ ENV ROS_WORKSPACE=/catkin_ws # Copy packages COPY ./ros_packages/ $ROS_WORKSPACE/src/ -RUN git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint $ROS_WORKSPACE/src/ros_tcp_endpoint -b v0.5.0 +RUN git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint $ROS_WORKSPACE/src/ros_tcp_endpoint -b laurie/NoMoreSourceDestinationDict COPY ./ros_docker/set-up-workspace /setup.sh #COPY docker/tutorial / From 9dfe04c4e534a5cf818fe427d01d76b3fb8b0c37 Mon Sep 17 00:00:00 2001 From: at669 Date: Mon, 27 Sep 2021 17:10:58 -0600 Subject: [PATCH 2/8] RealSim fixes, ROS script updates --- tutorials/pick_and_place/2_ros_tcp.md | 51 ++--- tutorials/pick_and_place/3_pick_and_place.md | 27 ++- .../PickAndPlaceProject/.gitignore | 1 + .../Assets/Tests/EditMode/IntegrationTest.cs | 6 +- .../Packages/manifest.json | 3 +- .../ROS/src/niryo_moveit/CMakeLists.txt | 1 - .../ROS/src/niryo_moveit/scripts/mover.py | 10 +- .../src/niryo_moveit/scripts/sim_real_pnp.py | 9 +- .../scripts/trajectory_subscriber.py | 2 +- .../Scripts_Part4/RealSimPickAndPlace.cs | 208 +++++++++--------- 10 files changed, 149 insertions(+), 169 deletions(-) diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index 8ed5bf0c..88634f56 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/tutorials/pick_and_place/2_ros_tcp.md @@ -87,33 +87,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(), - // 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() - }; - - // Place Pose - sourceDestinationMessage.place_pose = new PoseMsg - { - position = targetPlacement.transform.position.To(), - orientation = pickOrientation.To() - }; - - // 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(), + orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To() + }; + + // Place Pose + sourceDestinationMessage.place_pose = new PoseMsg + { + position = m_TargetPlacement.transform.position.To(), + orientation = m_PickOrientation.To() + }; + + // Finally send the message to server_endpoint.py running in ROS + m_Ros.Send(m_TopicName, sourceDestinationMessage); } ``` diff --git a/tutorials/pick_and_place/3_pick_and_place.md b/tutorials/pick_and_place/3_pick_and_place.md index 50925035..8bf5f70f 100644 --- a/tutorials/pick_and_place/3_pick_and_place.md +++ b/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(), + position = (m_Target.transform.position + m_PickPoseOffset).To(), + // 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() + orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To() }; // Place Pose request.place_pose = new PoseMsg { - position = (targetPlacement.transform.position + pickPoseOffset).To(), - orientation = pickOrientation.To() + position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To(), + orientation = m_PickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + m_Ros.SendServiceMessage(m_RosServiceName, request, TrajectoryResponse); } void TrajectoryResponse(MoverServiceResponse response) { - if (response.trajectories != null) + if (response.trajectories.Length > 0) { Debug.Log("Trajectory returned."); StartCoroutine(ExecuteTrajectories(response)); @@ -127,7 +128,7 @@ Steps covered in this tutorial includes invoking a motion planning service in RO > Note the file `src/niryo_moveit/scripts/mover.py`. This script holds the ROS-side logic for the MoverService. When the service is called, the function `plan_pick_and_place()` runs. This calls `plan_trajectory` on the current joint configurations (sent from Unity) to a destination pose (dependent on the phase of the pick-and-place task). ```python -def plan_trajectory(move_group, destination_pose, start_joint_angles): +def plan_trajectory(move_group, destination_pose, start_joint_angles): current_joint_state = JointState() current_joint_state.name = joint_names current_joint_state.position = start_joint_angles @@ -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. diff --git a/tutorials/pick_and_place/PickAndPlaceProject/.gitignore b/tutorials/pick_and_place/PickAndPlaceProject/.gitignore index f96bc123..28260e0d 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/.gitignore +++ b/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. diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs index 47e5a878..0e3f36be 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs @@ -123,9 +123,9 @@ void CreateTrajectoryPlannerPublisher() { var planner = new GameObject(k_NamePublisher).AddComponent(); planner.rosServiceName = k_NamePackageNiryoMoveIt; - planner.niryoOne = GameObject.Find(k_NameNiryoOne); - planner.target = GameObject.Find(k_NameTarget); - planner.targetPlacement = GameObject.Find(k_NameTargetPlacement); + planner.m_NiryoOne = GameObject.Find(k_NameNiryoOne); + planner.m_Target = GameObject.Find(k_NameTarget); + planner.m_TargetPlacement = GameObject.Find(k_NameTargetPlacement); } GameObject InstantiatePrefabFromName(string name) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index 49e6c37c..f780ee15 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -3,8 +3,7 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.8", "com.unity.ide.vscode": "1.2.3", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#laurie/VisualizationPackage", - "com.unity.robotics.visualizations": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations#laurie/VisualizationPackage", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#AIRO-1243-Unity-Service-Async", "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.5.0", "com.unity.test-framework": "1.1.24", "com.unity.textmeshpro": "3.0.6", diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/CMakeLists.txt b/tutorials/pick_and_place/ROS/src/niryo_moveit/CMakeLists.txt index 4abeb8bf..965d0ab2 100644 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/CMakeLists.txt +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/CMakeLists.txt @@ -31,7 +31,6 @@ catkin_package() catkin_install_python(PROGRAMS scripts/mover.py - scripts/server_endpoint.py scripts/trajectory_subscriber.py scripts/sim_real_pnp.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/mover.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/mover.py index 1689f672..575f1861 100755 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/mover.py +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/mover.py @@ -75,15 +75,7 @@ def plan_pick_and_place(req): group_name = "arm" move_group = moveit_commander.MoveGroupCommander(group_name) - # TODO: Make message type a list instead - current_robot_joint_configuration = [ - math.radians(req.joints_input.joint_00), - math.radians(req.joints_input.joint_01), - math.radians(req.joints_input.joint_02), - math.radians(req.joints_input.joint_03), - math.radians(req.joints_input.joint_04), - math.radians(req.joints_input.joint_05), - ] + current_robot_joint_configuration = req.joints_input.joints # Pre grasp - position gripper directly above target object pre_grasp_pose = plan_trajectory(move_group, req.pick_pose, current_robot_joint_configuration) diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/sim_real_pnp.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/sim_real_pnp.py index 47e72838..b6faa73e 100755 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/sim_real_pnp.py +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/sim_real_pnp.py @@ -126,14 +126,7 @@ def plan_pick_and_place(req): group_name = "arm" move_group = moveit_commander.MoveGroupCommander(group_name) - current_robot_joint_configuration = [ - math.radians(req.joints_input.joint_00), - math.radians(req.joints_input.joint_01), - math.radians(req.joints_input.joint_02), - math.radians(req.joints_input.joint_03), - math.radians(req.joints_input.joint_04), - math.radians(req.joints_input.joint_05), - ] + current_robot_joint_configuration = req.joints_input.joints # Pre grasp - position gripper directly above target object pre_grasp_pose = plan_trajectory(move_group, req.pick_pose, current_robot_joint_configuration) diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/trajectory_subscriber.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/trajectory_subscriber.py index ecb0607d..aad1d10d 100755 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/trajectory_subscriber.py +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/trajectory_subscriber.py @@ -15,7 +15,7 @@ def callback(data): def listener(): rospy.init_node('Trajectory_Subscriber', anonymous=True) - rospy.Subscriber("SourceDestination_input", NiryoMoveitJoints, callback) + rospy.Subscriber("/niryo_joints", NiryoMoveitJoints, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin() diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index 86b1005a..31c57479 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -1,3 +1,4 @@ +using System; using System.Collections; using System.Linq; using RosMessageTypes.Geometry; @@ -10,165 +11,159 @@ public class RealSimPickAndPlace : MonoBehaviour { - private const int OPEN_GRIPPER = 1; - private const int CLOSE_GRIPPER = 2; - private const int TOOL_COMMAND_EXECUTION = 6; - private const int TRAJECTORY_COMMAND_EXECUTION = 7; + const int k_OpenGripper = 1; + const int k_CloseGripper = 2; + const int k_ToolCommandExecution = 6; + const int k_TrajectoryCommandExecution = 7; + const int k_NumRobotJoints = 6; - private ROSConnection ros; - private const int NUM_ROBOT_JOINTS = 6; - - // Hardcoded variables - private const float JOINT_ASSIGNMENT_WAIT = 0.038f; - private readonly Vector3 PICK_POSE_OFFSET = Vector3.up * 0.15f; - - // Assures that the gripper is always positioned above the target cube before grasping. - private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); + // Hardcoded variables + const float k_JointAssignmentWait = 0.038f; // Variables required for ROS communication public string rosJointPublishTopicName = "sim_real_pnp"; public string rosRobotCommandsTopicName = "niryo_one/commander/robot_action/goal"; - public GameObject niryoOne; - public GameObject target; - public GameObject targetPlacement; + [SerializeField] + GameObject m_NiryoOne; + [SerializeField] + GameObject m_Target; + [SerializeField] + GameObject m_TargetPlacement; + + // Assures that the gripper is always positioned above the m_Target cube before grasping. + readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0); + readonly Vector3 m_PickPoseOffset = Vector3.up * 0.15f; // Articulation Bodies - private ArticulationBody[] jointArticulationBodies; - private ArticulationBody leftGripperJoint; - private ArticulationBody rightGripperJoint; + ArticulationBody[] m_JointArticulationBodies; + + ArticulationBody m_LeftGripper; + ArticulationBody m_RightGripper; - private Transform leftGripperGameObject; - private Transform rightGripperGameObject; + ROSConnection m_Ros; + + /// + /// Find all robot joints in Awake() and add them to the jointArticulationBodies array. + /// Find left and right finger joints and assign them to their respective articulation body objects. + /// + void Awake() + { + m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints]; + var linkName = "world/base_link/shoulder_link"; + m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/arm_link"; + m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/elbow_link"; + m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/forearm_link"; + m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/wrist_link"; + m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + linkName += "/hand_link"; + m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); + + // Find left and right fingers + var rightGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; + var leftGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper"; + + m_RightGripper = m_NiryoOne.transform.Find(rightGripper).GetComponent(); + m_LeftGripper = m_NiryoOne.transform.Find(leftGripper).GetComponent(); + } + + void Start() + { + // Get ROS connection static instance + m_Ros = ROSConnection.GetOrCreateInstance(); + m_Ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); + } /// /// Close the gripper /// - private void CloseGripper() + void CloseGripper() { - var leftDrive = leftGripperJoint.xDrive; - var rightDrive = rightGripperJoint.xDrive; + var leftDrive = m_LeftGripper.xDrive; + var rightDrive = m_RightGripper.xDrive; leftDrive.target = 0.01f; rightDrive.target = -0.01f; - leftGripperJoint.xDrive = leftDrive; - rightGripperJoint.xDrive = rightDrive; + m_LeftGripper.xDrive = leftDrive; + m_RightGripper.xDrive = rightDrive; } /// /// Open the gripper /// - private void OpenGripper() + void OpenGripper() { - var leftDrive = leftGripperJoint.xDrive; - var rightDrive = rightGripperJoint.xDrive; + var leftDrive = m_LeftGripper.xDrive; + var rightDrive = m_RightGripper.xDrive; leftDrive.target = -0.01f; rightDrive.target = 0.01f; - leftGripperJoint.xDrive = leftDrive; - rightGripperJoint.xDrive = rightDrive; + m_LeftGripper.xDrive = leftDrive; + m_RightGripper.xDrive = rightDrive; } - /// - /// Publish the robot's current joint configuration, the target object's - /// position and rotation, and the target placement for the target object's + /// Publish the robot's current joint configuration, the m_Target object's + /// position and rotation, and the m_Target placement for the m_Target object's /// position and rotation. - /// /// Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up /// public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest + var request = new MoverServiceRequest { - joints_input = - { - joint_00 = jointArticulationBodies[0].xDrive.target, - joint_01 = jointArticulationBodies[1].xDrive.target, - joint_02 = jointArticulationBodies[2].xDrive.target, - joint_03 = jointArticulationBodies[3].xDrive.target, - joint_04 = jointArticulationBodies[4].xDrive.target, - joint_05 = jointArticulationBodies[5].xDrive.target - }, + joints_input = new NiryoMoveitJointsMsg(), pick_pose = new PoseMsg { - position = (target.transform.position + PICK_POSE_OFFSET).To(), - // 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() + position = (m_Target.transform.position + m_PickPoseOffset).To(), + + // The hardcoded x/z angles assure that the gripper is always positioned above the m_Target cube before grasping. + orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To() }, place_pose = new PoseMsg { - position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To(), - orientation = pickOrientation.To() + position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To(), + orientation = m_PickOrientation.To() } }; - ros.Send(rosJointPublishTopicName, request); - } - - /// - /// Find all robot joints in Awake() and add them to the jointArticulationBodies array. - /// Find left and right finger joints and assign them to their respective articulation body objects. - /// - void Awake() - { - jointArticulationBodies = new ArticulationBody[NUM_ROBOT_JOINTS]; - string shoulder_link = "world/base_link/shoulder_link"; - jointArticulationBodies[0] = niryoOne.transform.Find(shoulder_link).GetComponent(); - - string armLink = shoulder_link + "/arm_link"; - jointArticulationBodies[1] = niryoOne.transform.Find(armLink).GetComponent(); + for (var i = 0; i < k_NumRobotJoints; i++) + request.joints_input.joints[i] = m_JointArticulationBodies[i].jointPosition[0]; - string elbowLink = armLink + "/elbow_link"; - jointArticulationBodies[2] = niryoOne.transform.Find(elbowLink).GetComponent(); - - string forearmLink = elbowLink + "/forearm_link"; - jointArticulationBodies[3] = niryoOne.transform.Find(forearmLink).GetComponent(); - - string wristLink = forearmLink + "/wrist_link"; - jointArticulationBodies[4] = niryoOne.transform.Find(wristLink).GetComponent(); - - string handLink = wristLink + "/hand_link"; - jointArticulationBodies[5] = niryoOne.transform.Find(handLink).GetComponent(); - - // Find left and right fingers - string rightGripper = handLink + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; - string leftGripper = handLink + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper"; - - leftGripperGameObject = niryoOne.transform.Find(leftGripper); - rightGripperGameObject = niryoOne.transform.Find(rightGripper); - - rightGripperJoint = rightGripperGameObject.GetComponent(); - leftGripperJoint = leftGripperGameObject.GetComponent(); - } - - void Start() - { - ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); + m_Ros.Send(rosJointPublishTopicName, request); } /// - /// Execute robot commands receved from ROS Subscriber. - /// Gripper commands will be executed immeditately wihle trajectories will be - /// executed in a coroutine. + /// Execute robot commands receved from ROS Subscriber. + /// Gripper commands will be executed immeditately wihle trajectories will be + /// executed in a coroutine. /// /// RobotMoveActionGoal of trajectory or gripper commands void ExecuteRobotCommands(RobotMoveActionGoal robotAction) { - if (robotAction.goal.cmd.cmd_type == TRAJECTORY_COMMAND_EXECUTION) + if (robotAction.goal.cmd.cmd_type == k_TrajectoryCommandExecution) { StartCoroutine(ExecuteTrajectories(robotAction.goal.cmd.Trajectory.trajectory)); } - else if (robotAction.goal.cmd.cmd_type == TOOL_COMMAND_EXECUTION) + else if (robotAction.goal.cmd.cmd_type == k_ToolCommandExecution) { - if (robotAction.goal.cmd.tool_cmd.cmd_type == OPEN_GRIPPER) + if (robotAction.goal.cmd.tool_cmd.cmd_type == k_OpenGripper) { Debug.Log("Open Tool Command"); OpenGripper(); } - else if (robotAction.goal.cmd.tool_cmd.cmd_type == CLOSE_GRIPPER) + else if (robotAction.goal.cmd.tool_cmd.cmd_type == k_CloseGripper) { Debug.Log("Close Tool Command"); CloseGripper(); @@ -178,29 +173,28 @@ void ExecuteRobotCommands(RobotMoveActionGoal robotAction) /// /// Execute trajectories from RobotMoveActionGoal topic. - /// - /// Execution will iterate through every robot pose in the trajectory pose + /// Execution will iterate through every robot pose in the trajectory pose /// array while updating the joint values on the simulated robot. - /// /// /// The array of poses for the robot to execute - private IEnumerator ExecuteTrajectories(RobotTrajectoryMsg trajectories) + IEnumerator ExecuteTrajectories(RobotTrajectoryMsg trajectories) { // For every robot pose in trajectory plan foreach (var point in trajectories.joint_trajectory.points) { var jointPositions = point.positions; - float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); + var result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); // Set the joint values for every joint - for (int joint = 0; joint < jointArticulationBodies.Length; joint++) + for (var joint = 0; joint < m_JointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = m_JointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; - jointArticulationBodies[joint].xDrive = joint1XDrive; + m_JointArticulationBodies[joint].xDrive = joint1XDrive; } + // Wait for robot to achieve pose for all joint assignments - yield return new WaitForSeconds(JOINT_ASSIGNMENT_WAIT); + yield return new WaitForSeconds(k_JointAssignmentWait); } } } From 773a5ad80604a9c50e1d0e8e27e82f2e9e011e98 Mon Sep 17 00:00:00 2001 From: at669 Date: Mon, 27 Sep 2021 17:17:22 -0600 Subject: [PATCH 3/8] Update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7266dec1..13f2fd36 100644 --- a/CHANGELOG.md +++ b/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 From 3dbeecdb0838932f8653143f4561013c4d209fb2 Mon Sep 17 00:00:00 2001 From: at669 Date: Mon, 27 Sep 2021 17:19:52 -0600 Subject: [PATCH 4/8] Pre-commit fixes --- tutorials/pick_and_place/3_pick_and_place.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/pick_and_place/3_pick_and_place.md b/tutorials/pick_and_place/3_pick_and_place.md index 8bf5f70f..a0b5a4b0 100644 --- a/tutorials/pick_and_place/3_pick_and_place.md +++ b/tutorials/pick_and_place/3_pick_and_place.md @@ -128,7 +128,7 @@ Steps covered in this tutorial includes invoking a motion planning service in RO > Note the file `src/niryo_moveit/scripts/mover.py`. This script holds the ROS-side logic for the MoverService. When the service is called, the function `plan_pick_and_place()` runs. This calls `plan_trajectory` on the current joint configurations (sent from Unity) to a destination pose (dependent on the phase of the pick-and-place task). ```python -def plan_trajectory(move_group, destination_pose, start_joint_angles): +def plan_trajectory(move_group, destination_pose, start_joint_angles): current_joint_state = JointState() current_joint_state.name = joint_names current_joint_state.position = start_joint_angles From 35a30aa3ff39e8181e1dd2f8c60ff3b52a7e35b9 Mon Sep 17 00:00:00 2001 From: at669 Date: Tue, 28 Sep 2021 11:36:33 -0600 Subject: [PATCH 5/8] PR feedback, demo fixes --- .../Assets/DemoScripts/Demo.cs | 243 +++++++++--------- .../Assets/Tests/EditMode/IntegrationTest.cs | 8 +- .../Scripts/SourceDestinationPublisher.cs | 27 +- .../Scripts/TrajectoryPlanner.cs | 32 +-- .../Scripts_Part4/RealSimPickAndPlace.cs | 58 ++--- 5 files changed, 180 insertions(+), 188 deletions(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 4ea42f98..37c52078 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -1,58 +1,70 @@ #if UNITY_EDITOR -using Microsoft.CSharp; using System; using System.CodeDom.Compiler; using System.Collections.Generic; using System.IO; using System.Reflection; using System.Text; -using UnityEditor; -using UnityEngine; +using Microsoft.CSharp; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.MessageGeneration; using Unity.Robotics.UrdfImporter; using Unity.Robotics.UrdfImporter.Control; +using UnityEditor; +using UnityEngine; public class Demo : MonoBehaviour { - public string hostIP = "127.0.0.1"; - - public bool generateRosMessages = true; - public bool deleteRosMessagesAfterSimulation = true; - - string prefabDirectory = "Assets/Prefabs"; - string prefabSuffix = ".prefab"; - - string tableName = "Table"; - string targetName = "Target"; - string targetPlacementName = "TargetPlacement"; - - string cameraName = "Main Camera"; - Vector3 cameraPosition = new Vector3(0, 1.4f, -0.7f); - Quaternion cameraRotation = Quaternion.Euler(new Vector3(45, 0, 0)); - - string urdfRelativeFilepath = "URDF/niryo_one/niryo_one.urdf"; - - string niryoOneName = "niryo_one"; - string baseLinkName = "base_link"; - float controllerStiffness = 10000; - float controllerDamping = 100; - float controllerForceLimit = 1000; - float controllerSpeed = 30; - float controllerAcceleration = 10; - - string rosMessagesDirectory = "Assets/Scripts/RosMessages"; - string rosSrcDirectory = "../ROS/src"; - string msgDirectory = "msg"; - string srvDirectory = "srv"; - string moveitMsgPackageName = "moveit_msgs"; - string niryoMoveitPackageName = "niryo_moveit"; - string robotTrajectoryMessageFileName = "RobotTrajectory.msg"; - string moverServiceFileName = "MoverService.srv"; - string scriptPattern = "*.cs"; - - string registerMethodName = "Register"; - string[] rosGeneratedTypeFullName = new string[] + const string k_BaseLinkName = "base_link"; + + const string k_CameraName = "Main Camera"; + const float k_ControllerAcceleration = 10; + const float k_ControllerDamping = 100; + const float k_ControllerForceLimit = 1000; + const float k_ControllerSpeed = 30; + const float k_ControllerStiffness = 10000; + + const string k_ExternalScriptsDirectory = "../Scripts"; + const int k_HostPort = 10000; + const string k_MoveitMsgPackageName = "moveit_msgs"; + const string k_MoverServiceFileName = "MoverService.srv"; + const string k_MsgDirectory = "msg"; + const string k_NiryoMoveitPackageName = "niryo_moveit"; + + const string k_NiryoOneName = "niryo_one"; + + const string k_PrefabDirectory = "Assets/Prefabs"; + const string k_PrefabSuffix = ".prefab"; + const string k_PublisherName = "Publisher"; + + const string k_RegisterMethodName = "Register"; + const string k_RobotTrajectoryMessageFileName = "RobotTrajectory.msg"; + + //string scriptsDirectory = "Assets/Scripts"; + + const string k_RosConnectName = "ROSConnect"; + + const string k_RosMessagesDirectory = "Assets/Scripts/RosMessages"; + const string k_RosServiceName = "niryo_moveit"; + const string k_RosSrcDirectory = "../ROS/src"; + const string k_ScriptPattern = "*.cs"; + const string k_SrvDirectory = "srv"; + + const string k_TableName = "Table"; + const string k_TargetName = "Target"; + const string k_TargetPlacementName = "TargetPlacement"; + + const string k_TrajectoryPlannerType = "TrajectoryPlanner"; + + const string k_UrdfRelativeFilepath = "URDF/niryo_one/niryo_one.urdf"; + [SerializeField] + string m_HostIP = "127.0.0.1"; + + [SerializeField] + bool m_GenerateRosMessages = true; + [SerializeField] + bool m_DeleteRosMessagesAfterSimulation = true; + readonly string[] m_RosGeneratedTypeFullName = { "RosMessageTypes.Moveit.RobotTrajectoryMsg", "RosMessageTypes.NiryoMoveit.NiryoMoveitJointsMsg", @@ -61,21 +73,12 @@ public class Demo : MonoBehaviour "RosMessageTypes.NiryoMoveit.MoverServiceRequest" }; + Assembly m_Assembly; + Vector3 m_CameraPosition = new Vector3(0, 1.4f, -0.7f); + Quaternion m_CameraRotation = Quaternion.Euler(new Vector3(45, 0, 0)); - string externalScriptsDirectory = "../Scripts"; - //string scriptsDirectory = "Assets/Scripts"; - - string rosConnectName = "ROSConnect"; - string publisherName = "Publisher"; - int hostPort = 10000; - - string trajectoryPlannerType = "TrajectoryPlanner"; - string rosServiceName = "niryo_moveit"; - - bool hasPublished = false; - - Assembly assembly; - ROSConnection rosConnection; + bool m_HasPublished; + ROSConnection m_RosConnection; void Awake() { @@ -90,150 +93,156 @@ void Awake() void Update() { // Make sure to publish only once in the demo - if (!hasPublished) + if (!m_HasPublished) { - dynamic publisher = GameObject.Find(publisherName).GetComponent(assembly.GetType(trajectoryPlannerType)); + dynamic publisher = GameObject.Find(k_PublisherName) + .GetComponent(m_Assembly.GetType(k_TrajectoryPlannerType)); publisher.PublishJoints(); - hasPublished = true; + m_HasPublished = true; } } void OnApplicationQuit() { - if (deleteRosMessagesAfterSimulation) + if (m_DeleteRosMessagesAfterSimulation) { - Directory.Delete(rosMessagesDirectory, recursive: true); - File.Delete($"{rosMessagesDirectory}.meta"); + Directory.Delete(k_RosMessagesDirectory, true); + File.Delete($"{k_RosMessagesDirectory}.meta"); } + EditorApplication.UnlockReloadAssemblies(); } // Tutorial Part 1 - private void SetupScene() + void SetupScene() { // Instantiate the table, target, and target placement - InstantiatePrefab(tableName); - InstantiatePrefab(targetName); - InstantiatePrefab(targetPlacementName); + InstantiatePrefab(k_TableName); + InstantiatePrefab(k_TargetName); + InstantiatePrefab(k_TargetPlacementName); // Adjust main camera - GameObject camera = GameObject.Find(cameraName); - camera.transform.position = cameraPosition; - camera.transform.rotation = cameraRotation; + var camera = GameObject.Find(k_CameraName); + camera.transform.position = m_CameraPosition; + camera.transform.rotation = m_CameraRotation; } // Tutorial Part 2 without the Publisher object - private void AddRosMessages() + void AddRosMessages() { - if (generateRosMessages) + if (m_GenerateRosMessages) { // Generate ROS messages MessageAutoGen.GenerateSingleMessage( - Path.Combine(rosSrcDirectory, moveitMsgPackageName, msgDirectory, robotTrajectoryMessageFileName), - rosMessagesDirectory, moveitMsgPackageName); + Path.Combine(k_RosSrcDirectory, k_MoveitMsgPackageName, k_MsgDirectory, + k_RobotTrajectoryMessageFileName), + k_RosMessagesDirectory, k_MoveitMsgPackageName); MessageAutoGen.GenerateDirectoryMessages( - Path.Combine(rosSrcDirectory, niryoMoveitPackageName, msgDirectory), - rosMessagesDirectory); + Path.Combine(k_RosSrcDirectory, k_NiryoMoveitPackageName, k_MsgDirectory), + k_RosMessagesDirectory); // Generate ROS services ServiceAutoGen.GenerateSingleService( - Path.Combine(rosSrcDirectory, niryoMoveitPackageName, srvDirectory, moverServiceFileName), - rosMessagesDirectory, niryoMoveitPackageName); + Path.Combine(k_RosSrcDirectory, k_NiryoMoveitPackageName, k_SrvDirectory, k_MoverServiceFileName), + k_RosMessagesDirectory, k_NiryoMoveitPackageName); } // Recompile ROS message scripts and external scripts - List scripts = new List(); - scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); - scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); + var scripts = new List(); + scripts.AddRange(Directory.GetFiles(k_RosMessagesDirectory, k_ScriptPattern, SearchOption.AllDirectories)); + scripts.AddRange(Directory.GetFiles(k_ExternalScriptsDirectory)); RecompileScripts(scripts.ToArray()); // Register Ros message names and their deserialize function - foreach (var typeFullName in rosGeneratedTypeFullName) + foreach (var typeFullName in m_RosGeneratedTypeFullName) { - assembly.GetType(typeFullName).GetMethod(registerMethodName).Invoke(null, null); + m_Assembly.GetType(typeFullName).GetMethod(k_RegisterMethodName).Invoke(null, null); } } - private void CreateTrajectoryPlannerPublisher() + void CreateTrajectoryPlannerPublisher() { - GameObject publisher = new GameObject(publisherName); - dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType)); - planner.rosServiceName = rosServiceName; - planner.niryoOne = GameObject.Find(niryoOneName); - planner.target = GameObject.Find(targetName); - planner.targetPlacement = GameObject.Find(targetPlacementName); + var publisher = new GameObject(k_PublisherName); + dynamic planner = publisher.AddComponent(m_Assembly.GetType(k_TrajectoryPlannerType)); + planner.RosServiceName = k_RosServiceName; + planner.NiryoOne = GameObject.Find(k_NiryoOneName); + planner.Target = GameObject.Find(k_TargetName); + planner.TargetPlacement = GameObject.Find(k_TargetPlacementName); } - private GameObject InstantiatePrefab(string name) + GameObject InstantiatePrefab(string name) { - string filepath = Path.Combine(prefabDirectory, $"{name}{prefabSuffix}"); - GameObject gameObject = Instantiate(AssetDatabase.LoadAssetAtPath(filepath)); + var filepath = Path.Combine(k_PrefabDirectory, $"{name}{k_PrefabSuffix}"); + var gameObject = Instantiate(AssetDatabase.LoadAssetAtPath(filepath)); gameObject.name = name; return gameObject; } - private void CreateRosConnection() + void CreateRosConnection() { // Create RosConnect - GameObject rosConnect = new GameObject(rosConnectName); - rosConnection = rosConnect.AddComponent(); - rosConnection.RosIPAddress = hostIP; - rosConnection.RosPort = hostPort; + var rosConnect = new GameObject(k_RosConnectName); + m_RosConnection = rosConnect.AddComponent(); + m_RosConnection.RosIPAddress = m_HostIP; + m_RosConnection.RosPort = k_HostPort; } - private void ImportRobot() + void ImportRobot() { - ImportSettings urdfImportSettings = new ImportSettings + var urdfImportSettings = new ImportSettings { choosenAxis = ImportSettings.axisType.yAxis, convexMethod = ImportSettings.convexDecomposer.unity }; + // Import Niryo One with URDF importer - string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); + var urdfFilepath = Path.Combine(Application.dataPath, k_UrdfRelativeFilepath); + // Create is a coroutine that would usually run only in EditMode, so we need to force its execution here - var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false); + var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings); while (robotImporter.MoveNext()) { } + // Adjust robot parameters - Controller controller = GameObject.Find(niryoOneName).GetComponent(); - controller.stiffness = controllerStiffness; - controller.damping = controllerDamping; - controller.forceLimit = controllerForceLimit; - controller.speed = controllerSpeed; - controller.acceleration = controllerAcceleration; - GameObject.Find(baseLinkName).GetComponent().immovable = true; + var controller = GameObject.Find(k_NiryoOneName).GetComponent(); + controller.stiffness = k_ControllerStiffness; + controller.damping = k_ControllerDamping; + controller.forceLimit = k_ControllerForceLimit; + controller.speed = k_ControllerSpeed; + controller.acceleration = k_ControllerAcceleration; + GameObject.Find(k_BaseLinkName).GetComponent().immovable = true; } // Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime // and https://gamedev.stackexchange.com/questions/130268/how-do-i-compile-a-c-script-at-runtime-and-attach-it-as-a-component-to-a-game-o - private void RecompileScripts(string[] filepaths) + void RecompileScripts(string[] filepaths) { - CSharpCodeProvider provider = new CSharpCodeProvider(); - CompilerParameters parameters = new CompilerParameters(); - foreach (Assembly unityAssembly in AppDomain.CurrentDomain.GetAssemblies()) + var provider = new CSharpCodeProvider(); + var parameters = new CompilerParameters(); + foreach (var unityAssembly in AppDomain.CurrentDomain.GetAssemblies()) { parameters.ReferencedAssemblies.Add(unityAssembly.Location); } parameters.GenerateInMemory = true; parameters.GenerateExecutable = false; - string[] texts = new string[filepaths.Length]; - for (int i = 0; i < filepaths.Length; i++) + var texts = new string[filepaths.Length]; + for (var i = 0; i < filepaths.Length; i++) { texts[i] = File.ReadAllText(filepaths[i]); } - CompilerResults results = provider.CompileAssemblyFromSource(parameters, texts); + var results = provider.CompileAssemblyFromSource(parameters, texts); checkCompileErrors(results); // Update the assembly - assembly = results.CompiledAssembly; + m_Assembly = results.CompiledAssembly; } - private void checkCompileErrors(CompilerResults results) + void checkCompileErrors(CompilerResults results) { if (results.Errors.HasErrors) { - StringBuilder stringBuilder = new StringBuilder(); + var stringBuilder = new StringBuilder(); foreach (CompilerError error in results.Errors) { stringBuilder.AppendLine($"Error {error.ErrorNumber} {error.ErrorText}"); diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs index 0e3f36be..857a07b3 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/IntegrationTest.cs @@ -122,10 +122,10 @@ void SetUpScene() void CreateTrajectoryPlannerPublisher() { var planner = new GameObject(k_NamePublisher).AddComponent(); - planner.rosServiceName = k_NamePackageNiryoMoveIt; - planner.m_NiryoOne = GameObject.Find(k_NameNiryoOne); - planner.m_Target = GameObject.Find(k_NameTarget); - planner.m_TargetPlacement = GameObject.Find(k_NameTargetPlacement); + planner.RosServiceName = k_NamePackageNiryoMoveIt; + planner.NiryoOne = GameObject.Find(k_NameNiryoOne); + planner.Target = GameObject.Find(k_NameTarget); + planner.TargetPlacement = GameObject.Find(k_NameTargetPlacement); } GameObject InstantiatePrefabFromName(string name) diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index 8798cefb..42df27a3 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -10,6 +10,9 @@ public class SourceDestinationPublisher : MonoBehaviour { const int k_NumRobotJoints = 6; + public static readonly string[] LinkNames = + { "world/base_link/shoulder_link", "/arm_link", "/elbow_link", "/forearm_link", "/wrist_link", "/hand_link" }; + // Variables required for ROS communication [SerializeField] string m_TopicName = "/niryo_joints"; @@ -35,23 +38,13 @@ void Start() m_Ros.RegisterPublisher(m_TopicName); m_JointArticulationBodies = new UrdfJointRevolute[k_NumRobotJoints]; - var linkName = "world/base_link/shoulder_link"; - m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/arm_link"; - m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/elbow_link"; - m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/forearm_link"; - m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); - linkName += "/wrist_link"; - m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/hand_link"; - m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); + var linkName = string.Empty; + for (var i = 0; i < k_NumRobotJoints; i++) + { + linkName += LinkNames[i]; + m_JointArticulationBodies[i] = m_NiryoOne.transform.Find(linkName).GetComponent(); + } } public void Publish() @@ -78,6 +71,6 @@ public void Publish() }; // Finally send the message to server_endpoint.py running in ROS - m_Ros.Send(m_TopicName, sourceDestinationMessage); + m_Ros.Publish(m_TopicName, sourceDestinationMessage); } } diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index 8c3679dd..b4c8526d 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -17,15 +17,19 @@ public class TrajectoryPlanner : MonoBehaviour // Variables required for ROS communication [SerializeField] string m_RosServiceName = "niryo_moveit"; + public string RosServiceName { get => m_RosServiceName; set => m_RosServiceName = value; } [SerializeField] GameObject m_NiryoOne; + public GameObject NiryoOne { get => m_NiryoOne; set => m_NiryoOne = value; } [SerializeField] GameObject m_Target; + public GameObject Target { get => m_Target; set => m_Target = value; } [SerializeField] GameObject m_TargetPlacement; + public GameObject TargetPlacement { get => m_TargetPlacement; set => m_TargetPlacement = value; } - // Assures that the gripper is always positioned above the target cube before grasping. + // Assures that the gripper is always positioned above the m_Target cube before grasping. readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0); readonly Vector3 m_PickPoseOffset = Vector3.up * 0.1f; @@ -48,23 +52,13 @@ void Start() m_Ros.RegisterRosService(m_RosServiceName); m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints]; - var linkName = "world/base_link/shoulder_link"; - m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); - linkName += "/arm_link"; - m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/elbow_link"; - m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/forearm_link"; - m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/wrist_link"; - m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/hand_link"; - m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); + var linkName = string.Empty; + for (var i = 0; i < k_NumRobotJoints; i++) + { + linkName += SourceDestinationPublisher.LinkNames[i]; + m_JointArticulationBodies[i] = m_NiryoOne.transform.Find(linkName).GetComponent(); + } // Find left and right fingers var rightGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; @@ -200,8 +194,10 @@ IEnumerator ExecuteTrajectories(MoverServiceResponse response) } // Close the gripper if completed executing the trajectory for the Grasp pose - if (poseIndex == (int)Poses.Grasp) + if (poseIndex == (int)Poses.Grasp) + { CloseGripper(); + } // Wait for the robot to achieve the final pose from joint assignment yield return new WaitForSeconds(k_PoseAssignmentWait); diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index 31c57479..5fa4aaaa 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -50,23 +50,13 @@ public class RealSimPickAndPlace : MonoBehaviour void Awake() { m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints]; - var linkName = "world/base_link/shoulder_link"; - m_JointArticulationBodies[0] = m_NiryoOne.transform.Find(linkName).GetComponent(); - linkName += "/arm_link"; - m_JointArticulationBodies[1] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/elbow_link"; - m_JointArticulationBodies[2] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/forearm_link"; - m_JointArticulationBodies[3] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/wrist_link"; - m_JointArticulationBodies[4] = m_NiryoOne.transform.Find(linkName).GetComponent(); - - linkName += "/hand_link"; - m_JointArticulationBodies[5] = m_NiryoOne.transform.Find(linkName).GetComponent(); + var linkName = string.Empty; + for (var i = 0; i < k_NumRobotJoints; i++) + { + linkName += SourceDestinationPublisher.LinkNames[i]; + m_JointArticulationBodies[i] = m_NiryoOne.transform.Find(linkName).GetComponent(); + } // Find left and right fingers var rightGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; @@ -139,9 +129,11 @@ public void PublishJoints() }; for (var i = 0; i < k_NumRobotJoints; i++) + { request.joints_input.joints[i] = m_JointArticulationBodies[i].jointPosition[0]; + } - m_Ros.Send(rosJointPublishTopicName, request); + m_Ros.Publish(rosJointPublishTopicName, request); } /// @@ -152,22 +144,24 @@ public void PublishJoints() /// RobotMoveActionGoal of trajectory or gripper commands void ExecuteRobotCommands(RobotMoveActionGoal robotAction) { - if (robotAction.goal.cmd.cmd_type == k_TrajectoryCommandExecution) - { - StartCoroutine(ExecuteTrajectories(robotAction.goal.cmd.Trajectory.trajectory)); - } - else if (robotAction.goal.cmd.cmd_type == k_ToolCommandExecution) + switch (robotAction.goal.cmd.cmd_type) { - if (robotAction.goal.cmd.tool_cmd.cmd_type == k_OpenGripper) - { - Debug.Log("Open Tool Command"); - OpenGripper(); - } - else if (robotAction.goal.cmd.tool_cmd.cmd_type == k_CloseGripper) - { - Debug.Log("Close Tool Command"); - CloseGripper(); - } + case k_TrajectoryCommandExecution: + StartCoroutine(ExecuteTrajectories(robotAction.goal.cmd.Trajectory.trajectory)); + break; + case k_ToolCommandExecution: + switch (robotAction.goal.cmd.tool_cmd.cmd_type) + { + case k_OpenGripper: + Debug.Log("Open Tool Command"); + OpenGripper(); + break; + case k_CloseGripper: + Debug.Log("Close Tool Command"); + CloseGripper(); + break; + } + break; } } From 1524c027ca6ea5dad5b199b865c8779cb35ffeee Mon Sep 17 00:00:00 2001 From: at669 Date: Tue, 28 Sep 2021 11:39:46 -0600 Subject: [PATCH 6/8] Pre-commit fixes --- tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index b4c8526d..784e7569 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -194,7 +194,7 @@ IEnumerator ExecuteTrajectories(MoverServiceResponse response) } // Close the gripper if completed executing the trajectory for the Grasp pose - if (poseIndex == (int)Poses.Grasp) + if (poseIndex == (int)Poses.Grasp) { CloseGripper(); } From 905015bd451e448df6e50b0445205dcdf024746b Mon Sep 17 00:00:00 2001 From: at669 Date: Thu, 30 Sep 2021 16:37:17 -0600 Subject: [PATCH 7/8] Tutorial updates --- tutorials/pick_and_place/0_ros_setup.md | 2 +- tutorials/pick_and_place/2_ros_tcp.md | 2 -- .../pick_and_place/PickAndPlaceProject/Packages/manifest.json | 2 +- tutorials/pick_and_place/ROS/src/ros_tcp_endpoint | 2 +- tutorials/ros_unity_integration/ros_docker/Dockerfile | 3 +-- 5 files changed, 4 insertions(+), 7 deletions(-) diff --git a/tutorials/pick_and_place/0_ros_setup.md b/tutorials/pick_and_place/0_ros_setup.md index bd95617d..ad785cab 100644 --- a/tutorials/pick_and_place/0_ros_setup.md +++ b/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#`. diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index 88634f56..a4d50fa0 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/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. diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index f780ee15..7181c1cd 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -3,7 +3,7 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.8", "com.unity.ide.vscode": "1.2.3", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#AIRO-1243-Unity-Service-Async", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#dev", "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.5.0", "com.unity.test-framework": "1.1.24", "com.unity.textmeshpro": "3.0.6", diff --git a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint index fce165aa..dd4563cb 160000 --- a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint +++ b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint @@ -1 +1 @@ -Subproject commit fce165aa00057936df589274e242c11f2b90488c +Subproject commit dd4563cb2c9d9376d3f3f0e1602bd76ec355dafc diff --git a/tutorials/ros_unity_integration/ros_docker/Dockerfile b/tutorials/ros_unity_integration/ros_docker/Dockerfile index 3eeb644a..baabefe1 100644 --- a/tutorials/ros_unity_integration/ros_docker/Dockerfile +++ b/tutorials/ros_unity_integration/ros_docker/Dockerfile @@ -5,8 +5,7 @@ ENV ROS_WORKSPACE=/catkin_ws # Copy packages COPY ./ros_packages/ $ROS_WORKSPACE/src/ -RUN git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint $ROS_WORKSPACE/src/ros_tcp_endpoint -b laurie/NoMoreSourceDestinationDict - +RUN git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint $ROS_WORKSPACE/src/ros_tcp_endpoint -b dev COPY ./ros_docker/set-up-workspace /setup.sh #COPY docker/tutorial / From 39088cd48c2d4697a924563dd0bd20a987401574 Mon Sep 17 00:00:00 2001 From: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 30 Sep 2021 16:43:04 -0700 Subject: [PATCH 8/8] Updated scripts to run 0.6.0 --- tutorials/ros_unity_integration/publisher.md | 4 ++-- tutorials/ros_unity_integration/service_call.md | 6 +++--- tutorials/ros_unity_integration/subscriber.md | 2 +- .../unity_scripts/RosPublisherExample.cs | 4 ++-- .../unity_scripts/RosServiceCallExample.cs | 6 +++--- .../unity_scripts/RosSubscriberExample.cs | 2 +- .../unity_scripts/RosUnityServiceExample.cs | 2 +- tutorials/ros_unity_integration/unity_service.md | 2 +- 8 files changed, 14 insertions(+), 14 deletions(-) diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index 9c3421b3..27de4f66 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -36,7 +36,7 @@ public class RosPublisherExample : MonoBehaviour void Start() { // start the ROS connection - ros = ROSConnection.instance; + ros = ROSConnection.GetOrCreateInstance(); ros.RegisterPublisher(topicName); } @@ -59,7 +59,7 @@ public class RosPublisherExample : MonoBehaviour ); // Finally send the message to server_endpoint.py running in ROS - ros.Send(topicName, cubePos); + ros.Publish(topicName, cubePos); timeElapsed = 0; } diff --git a/tutorials/ros_unity_integration/service_call.md b/tutorials/ros_unity_integration/service_call.md index 299737b1..cb399927 100644 --- a/tutorials/ros_unity_integration/service_call.md +++ b/tutorials/ros_unity_integration/service_call.md @@ -30,9 +30,9 @@ Create a simple Unity scene which calls an external [ROS service](http://wiki.ro - (Alternatively, you can drag the script file into Unity from `tutorials/ros_unity_integration/unity_scripts`). ```csharp +using RosMessageTypes.UnityRoboticsDemo; using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosMessageTypes.UnityRoboticsDemo; public class RosServiceCallExample : MonoBehaviour { @@ -51,8 +51,8 @@ public class RosServiceCallExample : MonoBehaviour void Start() { - ros = ROSConnection.instance; - ros.RegisterRosService(serviceName); + ros = ROSConnection.GetOrCreateInstance(); + ros.RegisterRosService(serviceName); destination = cube.transform.position; } diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index 5efea233..4bb40199 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -22,7 +22,7 @@ public class RosSubscriberExample : MonoBehaviour void Start() { - ROSConnection.instance.Subscribe("color", ColorChange); + ROSConnection.GetOrCreateInstance().Subscribe("color", ColorChange); } void ColorChange(RosColor colorMessage) diff --git a/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs index c65452be..664634a2 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs @@ -21,7 +21,7 @@ public class RosPublisherExample : MonoBehaviour void Start() { // start the ROS connection - ros = ROSConnection.instance; + ros = ROSConnection.GetOrCreateInstance(); ros.RegisterPublisher(topicName); } @@ -44,7 +44,7 @@ private void Update() ); // Finally send the message to server_endpoint.py running in ROS - ros.Send(topicName, cubePos); + ros.Publish(topicName, cubePos); timeElapsed = 0; } diff --git a/tutorials/ros_unity_integration/unity_scripts/RosServiceCallExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosServiceCallExample.cs index ea15ed96..7a9c30f6 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosServiceCallExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosServiceCallExample.cs @@ -2,7 +2,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; -public class RosServiceExample : MonoBehaviour +public class RosServiceCallExample : MonoBehaviour { ROSConnection ros; @@ -19,8 +19,8 @@ public class RosServiceExample : MonoBehaviour void Start() { - ros = ROSConnection.instance; - ros.RegisterRosService(serviceName); + ros = ROSConnection.GetOrCreateInstance(); + ros.RegisterRosService(serviceName); destination = cube.transform.position; } diff --git a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs index 4e1addd9..d4e88f9b 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs @@ -8,7 +8,7 @@ public class RosSubscriberExample : MonoBehaviour void Start() { - ROSConnection.instance.Subscribe("color", ColorChange); + ROSConnection.GetOrCreateInstance().Subscribe("color", ColorChange); } void ColorChange(RosColor colorMessage) diff --git a/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs index 76ef4296..ce59d8d3 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosUnityServiceExample.cs @@ -14,7 +14,7 @@ public class RosUnityServiceExample : MonoBehaviour void Start() { // register the service with ROS - ROSConnection.instance.ImplementService(m_ServiceName, GetObjectPose); + ROSConnection.GetOrCreateInstance().ImplementService(m_ServiceName, GetObjectPose); } /// diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md index 68821934..e13626a0 100644 --- a/tutorials/ros_unity_integration/unity_service.md +++ b/tutorials/ros_unity_integration/unity_service.md @@ -28,7 +28,7 @@ public class RosUnityServiceExample : MonoBehaviour void Start() { // register the service with ROS - ROSConnection.instance.ImplementService(m_ServiceName, GetObjectPose); + ROSConnection.GetOrCreateInstance().ImplementService(m_ServiceName, GetObjectPose); } ///