/// <summary>
/// Create a new PoseEstimationServiceRequest with the captured screenshot as bytes and instantiates
/// a sensor_msgs/image.
///
/// Call the PoseEstimationService using the ROSConnection and calls PoseEstimationCallback on the
/// PoseEstimationServiceResponse.
/// </summary>
/// <param name="imageData"></param>
private void InvokePoseEstimationService(byte[] imageData)
{
uint imageHeight = (uint)renderTexture.height;
uint imageWidth = (uint)renderTexture.width;
RosMessageTypes.Sensor.Image rosImage = new RosMessageTypes.Sensor.Image(new RosMessageTypes.Std.Header(), imageWidth, imageHeight, "RGBA", isBigEndian, step, imageData);
PoseEstimationServiceRequest poseServiceRequest = new PoseEstimationServiceRequest(rosImage);
ros.SendServiceMessage <PoseEstimationServiceResponse>("pose_estimation_srv", poseServiceRequest, PoseEstimationCallback);
}
private void Update()
{
// Move our position a step closer to the target.
float step = speed * Time.deltaTime; // calculate distance to move
cube.transform.position = Vector3.MoveTowards(cube.transform.position, destination, step);
if (Vector3.Distance(cube.transform.position, destination) < delta && Time.time > awaitingResponseUntilTimestamp)
{
Debug.Log("Destination reached.");
PosRot cubePos = new PosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
// Send message to ROS and return the response
ros.SendServiceMessage <PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
awaitingResponseUntilTimestamp = Time.time + 1.0f; // don't send again for 1 second, or until we receive a response
}
}
/// <summary>
/// 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.
/// </summary>
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
target.transform.position.z,
-target.transform.position.x,
// Add pick pose offset to position the gripper above target to avoid collisions
target.transform.position.y + pickPoseOffset
),
// Orientation is hardcoded for this example so the gripper is always directly above the target object
orientation = pickOrientation
};
// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
targetPlacement.transform.position.z,
-targetPlacement.transform.position.x,
// Use the same pick pose offset so the target cube can be seen dropping into position
targetPlacement.transform.position.y + pickPoseOffset
),
// Orientation is hardcoded for this example so the gripper is always directly above the target object
orientation = pickOrientation
};
ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
/// <summary>
/// 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.
/// </summary>
public void PublishJoints()
{
GotoServiceRequest request = new GotoServiceRequest();
request.joints_input = CurrentJointConfig();
if (targetX == 0 && targetY == 0 && targetZ == 0)
{
request.target_pose = new RosMessageTypes.Geometry.Pose
{
position = (target.transform.position + 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>()
};
}
else
{
float Xpose = targetX * 0.3f;
float Ypose = (targetY * 0.15f) + 0.75f;
float Zpose = targetZ * 0.225f;
request.target_pose = new RosMessageTypes.Geometry.Pose
{
position = (new Vector3(Xpose, Ypose, Zpose) + 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>()
};
}
// Pick Pose
ros.SendServiceMessage <GotoServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
public void Invoke()
{
ArmPose pose = arm.GetArmPose();
JointState[] joints = new[]
{
pose.world_joint,
pose.base_joint,
pose.shoulder_joint,
pose.elbow_joint,
pose.wrist_joint,
pose.eff_joint,
pose.gripper_offset_joint
};
ForwardKinematicsRequest req = new ForwardKinematicsRequest(joints);
ros.SendServiceMessage <ForwardKinematicsResponse>("forward_kinematics", req, Callback);
}
public void Invoke()
{
ArmPose pose = arm.GetArmPose();
var pos = arm.worldJoint.transform.InverseTransformPoint(target.transform.position);
Debug.Log(pos);
TrajectoryPlannerRequest req = new TrajectoryPlannerRequest(
pose,
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
pos.x,
pos.y,
pos.z),
new RosMessageTypes.Geometry.Quaternion(0, 0, 0, 0)),
true,
false);
ros.SendServiceMessage <TrajectoryPlannerResponse>("trajectory_planner", req, Callback);
}
public void Invoke()
{
ArmPose arm_pose = arm.GetArmPose();
Vector3 wPos = WristTarget.transform.localPosition;
Quaternion wRot = WristTarget.transform.localRotation;
Vector3 ePos = arm.worldJoint.transform.InverseTransformPoint(EffTarget.transform.position);
Quaternion eRot = EffTarget.transform.localRotation;
InverseKinematicsRequest req = new InverseKinematicsRequest(
arm_pose,
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
wPos.x, wPos.y, wPos.z),
new RosMessageTypes.Geometry.Quaternion(
wRot.x, wRot.y, wRot.z, wRot.w)),
new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(
ePos.x, ePos.y, ePos.z),
new RosMessageTypes.Geometry.Quaternion(
eRot.x, eRot.y, eRot.z, eRot.w)));
ros.SendServiceMessage <InverseKinematicsResponse>("inverse_kinematics", req, Callback);
}
/// <summary>
/// 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.
/// </summary>
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new RosMessageTypes.Geometry.Pose
{
position = (target.transform.position + 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>()
};
// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
{
position = (targetPlacement.transform.position + pickPoseOffset).To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
public void Invoke()
{
var req = new StartSystemRequest(true);
ros.SendServiceMessage <StartSystemResponse>("start_system", req, Callback);
}