public void Publish()
{
MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints();
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 MPose
{
position = target.transform.position.To <FLU>(),
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
};
// Place Pose
sourceDestinationMessage.place_pose = new MPose
{
position = targetPlacement.transform.position.To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, sourceDestinationMessage);
}
/// <summary>
/// Get the current values of the robot's joint angles.
/// </summary>
/// <returns>NiryoMoveitJoints</returns>
MNiryoMoveitJoints CurrentJointConfig()
{
MNiryoMoveitJoints joints = new MNiryoMoveitJoints();
joints.joint_00 = jointArticulationBodies[0].xDrive.target;
joints.joint_01 = jointArticulationBodies[1].xDrive.target;
joints.joint_02 = jointArticulationBodies[2].xDrive.target;
joints.joint_03 = jointArticulationBodies[3].xDrive.target;
joints.joint_04 = jointArticulationBodies[4].xDrive.target;
joints.joint_05 = jointArticulationBodies[5].xDrive.target;
return(joints);
}
