public void Publish() { NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints(); 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 RosMessageTypes.Geometry.Pose { position = target.transform.position.To <FLU>(), orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>() }; // Place Pose sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose { 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> NiryoMoveitJoints CurrentJointConfig() { NiryoMoveitJoints joints = new NiryoMoveitJoints(); 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); }