public void Publish()
{
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 <FLU>(),
orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To <FLU>()
};
// Place Pose
sourceDestinationMessage.place_pose = new PoseMsg
{
position = m_TargetPlacement.transform.position.To <FLU>(),
orientation = m_PickOrientation.To <FLU>()
};
// Finally send the message to server_endpoint.py running in ROS
m_Ros.Publish(m_TopicName, sourceDestinationMessage);
}
/// <summary>
/// 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
/// </summary>
public void PublishJoints()
{
var request = new MoverServiceRequest
{
joints_input = new NiryoMoveitJointsMsg(),
pick_pose = new PoseMsg
{
position = (m_Target.transform.position + m_PickPoseOffset).To <FLU>(),
// 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 <FLU>()
},
place_pose = new PoseMsg
{
position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To <FLU>(),
orientation = m_PickOrientation.To <FLU>()
}
};
for (var i = 0; i < k_NumRobotJoints; i++)
{
request.joints_input.joints[i] = m_JointArticulationBodies[i].jointPosition[0];
}
m_Ros.Publish(rosJointPublishTopicName, request);
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
cube.transform.rotation = Random.rotation;
PosRotMsg cubePos = new PosRotMsg(
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
);
// Finally send the message to server_endpoint.py running in ROS
ros.Publish(topicName, cubePos);
timeElapsed = 0;
}
}
public void GoalPublish()
{
to_test = false;
ros.Publish(topicName, this._goal);
}
public virtual void PublishData(ROSBridgeMsg msg)
{
ROSConnection.Publish(TopicName, msg);
}
