First, I want to send a string type data service request from Unity to the pose estimation server, then receive the pose estimation return. In the pose estimation callback function, I will send the pose estimation return to the MoveIt！ service server, which will return the robotic arm’s planned trajectory, and then execute it in Unity.`using System;
using System.Collections;
using System.Linq;
using RosMessageTypes.Geometry;
using RosMessageTypes.RokaeWithGripperMoveitConfig;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using UnityEngine;
using UnityEngine.Perception.GroundTruth;
using RosMessageTypes.Std;
public class Book_PoseEstimate : MonoBehaviour
{
public PerceptionCamera perceptionCamera;
int index = 0;
public string images_path = “/home/jimmy/.config/unity3d/UnityRobotics/PickAndPlace/solo/sqeuence.0/step_”;
// Hardcoded variables
const int k_NumRobotJoints = 7;
const float k_JointAssignmentWait = 0.2f;
const float k_PoseAssignmentWait = 0.25f;
public static readonly string LinkNames = { “world/xMatePro7_base/xMatePro7_link1”, “/xMatePro7_link2”, “/xMatePro7_link3”, “/xMatePro7_link4”, “/xMatePro7_link5”, “/xMatePro7_link6”, “/xMatePro7_link7”};
// Variables required for ROS communication
[SerializeField]
string m_RosServiceName = “rokae_moveit”;
public string RosServiceName { get => m_RosServiceName; set => m_RosServiceName = value; }
[SerializeField]
GameObject m_Rokae;
public GameObject Rokae { get => m_Rokae; set => m_Rokae = 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 m_Target cube before grasping.
readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0);
readonly Vector3 m_PickPoseOffset = Vector3.up * 0.19f;
// Articulation Bodies
ArticulationBody[] m_JointArticulationBodies;
ArticulationBody m_LeftGripper;
ArticulationBody m_RightGripper;
// ROS Connector
ROSConnection m_Ros;
void Start()
{
// Get ROS connection static instance
m_Ros = ROSConnection.GetOrCreateInstance();
// m_Ros.RegisterRosService<MoverServiceRequest, MoverServiceResponse>(m_RosServiceName);
m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints];
var linkName = string.Empty;
for (var i = 0; i < k_NumRobotJoints; i++)
{
linkName += LinkNames[i];
m_JointArticulationBodies[i] = m_Rokae.transform.Find(linkName).GetComponent<ArticulationBody>();
}
// Find left and right fingers
var rightGripper = linkName + "/base_link/finger1_link";
var leftGripper = linkName + "/base_link/finger2_link";
m_RightGripper = m_Rokae.transform.Find(rightGripper).GetComponent<ArticulationBody>();
m_LeftGripper = m_Rokae.transform.Find(leftGripper).GetComponent<ArticulationBody>();
}
void CloseGripper()
{
var leftDrive = m_LeftGripper.xDrive;
var rightDrive = m_RightGripper.xDrive;
leftDrive.target = 0.025f;
leftDrive.targetVelocity = 0.0f;
rightDrive.target = 0.025f;
rightDrive.targetVelocity = 0.0f;
m_LeftGripper.xDrive = leftDrive;
m_RightGripper.xDrive = rightDrive;
}
void OpenGripper()
{
var leftDrive = m_LeftGripper.xDrive;
var rightDrive = m_RightGripper.xDrive;
leftDrive.target = 0.0f;
rightDrive.target = -0.0f;
m_LeftGripper.xDrive = leftDrive;
m_RightGripper.xDrive = rightDrive;
}
RokaeMoveitJointsMsgMsg CurrentJointConfig()
{
var joints = new RokaeMoveitJointsMsgMsg();
for (var i = 0; i < k_NumRobotJoints; i++)
{
joints.joints[i] = m_JointArticulationBodies[i].jointPosition[0];
}
return joints;
}
public void PoseEstimation()
{
Debug.Log($"press 's' start Perception Camera get {index} frame...");
images_path = string.Concat(images_path, index);
PoseEstimationServiceRequest request = new PoseEstimationServiceRequest();
request.image_path = images_path;
// Debug.Log(request);
m_Ros.SendServiceMessage<PoseEstimationServiceResponse>("pose_estimation_service", request, PoseEstimationCallback);
Debug.Log("send images_path to ROS service...");
}
void PoseEstimationCallback(PoseEstimationServiceResponse response)
{
if (response != null)
{
Debug.Log("Pose Estimation Service successfully returned pose estimation result!");
var estimatedPosition = Camera.main.transform.TransformPoint(response.estimated_pose.position.From<RUF>());
var estimatedRotation = Camera.main.transform.rotation * response.estimated_pose.orientation.From<RUF>();
PublishJoints(estimatedPosition, estimatedRotation);
}
Debug.Log("Pose Estimation Service failed to return pose estimation result!");
}
public void PublishJoints(Vector3 position, Quaternion rotation)
{
var request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new PoseMsg
{
position = (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(180, rotation.eulerAngles.y-180, 0)).To<FLU>()
};
// Place Pose
request.place_pose = new PoseMsg
{
position = (m_TargetPlacement.transform.position + m_PickPoseOffset * 2).To<FLU>(),
orientation = (Quaternion.Euler(180, m_TargetPlacement.transform.eulerAngles.y-90, 0) ).To<FLU>()
};
// Debug.Log(request);
m_Ros.SendServiceMessage<MoverServiceResponse>(m_RosServiceName, request, TrajectoryResponse);
}
void TrajectoryResponse(MoverServiceResponse response)
{
if (response.trajectories.Length > 0)
{
Debug.Log("Trajectory returned.");
StartCoroutine(ExecuteTrajectories(response));
}
else
{
Debug.LogError("No trajectory returned from MoverService.");
}
}
IEnumerator ExecuteTrajectories(MoverServiceResponse response)
{
if (response.trajectories != null)
{
// For every trajectory plan returned
for (var poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++)
{
// For every robot pose in trajectory plan
foreach (var t in response.trajectories[poseIndex].joint_trajectory.points)
{
var jointPositions = t.positions;
var result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
// Set the joint values for every joint
for (var joint = 0; joint < m_JointArticulationBodies.Length; joint++)
{
var joint1XDrive = m_JointArticulationBodies[joint].xDrive;
joint1XDrive.target = result[joint];
m_JointArticulationBodies[joint].xDrive = joint1XDrive;
}
// Wait for robot to achieve pose for all joint assignments
yield return new WaitForSeconds(k_JointAssignmentWait);
}
// Close the gripper if completed executing the trajectory for the Grasp pose
if (poseIndex == (int)Poses.Grasp)
{
yield return new WaitForSeconds(k_JointAssignmentWait);
// yield return new WaitForSeconds(2.0f);
CloseGripper();
yield return new WaitForSeconds(2.0f);
}
// Wait for the robot to achieve the final pose from joint assignment
yield return new WaitForSeconds(k_PoseAssignmentWait);
}
// All trajectories have been executed, open the gripper to place the target cube
OpenGripper();
}
}
enum Poses
{
PreGrasp,
Grasp,
PickUp,
Place
}
}
`
And the button settings are shown above.