public void publish(String messageType, long uid, long robotTimestamp, long lastReceivedTimestamp) { ihmc_msgs.LastReceivedMessage message = getMessage(); message.setType(messageType); message.setUniqueId(uid); message.setReceiveTimestamp(lastReceivedTimestamp); message.setTimeSinceLastReceived(robotTimestamp - lastReceivedTimestamp); publish(message); }
public void publish(Point2d[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX((float) points[i].getX()); point2d.setY((float) points[i].getY()); vertices.add(point2d); } publish(message); }
private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message) throws Exception { WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage(); wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId()); wholeBodyTrajectoryMessage.leftArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage()); wholeBodyTrajectoryMessage.rightArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage()); wholeBodyTrajectoryMessage.leftHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage()); wholeBodyTrajectoryMessage.rightHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage()); wholeBodyTrajectoryMessage.leftFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage()); wholeBodyTrajectoryMessage.rightFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage()); wholeBodyTrajectoryMessage.chestTrajectoryMessage = (ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage()); wholeBodyTrajectoryMessage.pelvisTrajectoryMessage = (PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage()); return wholeBodyTrajectoryMessage; }
private static Packet customConvertToIHMCMessage(FootstepDataListRosMessage message) throws Exception { FootstepDataListMessage footsteps = new FootstepDataListMessage(); footsteps.defaultSwingTime = message.getDefaultSwingTime(); footsteps.defaultTransferTime = message.getDefaultTransferTime(); footsteps.setUniqueId(message.getUniqueId()); footsteps.executionMode = ExecutionMode.values[message.getExecutionMode()]; footsteps.finalTransferTime = message.getFinalTransferTime(); ArrayList<FootstepDataMessage> stepData = new ArrayList<>(); for (FootstepDataRosMessage footstepDataRosMessage : message.getFootstepDataList()) { stepData.add((FootstepDataMessage) convertToIHMCMessage(footstepDataRosMessage)); } footsteps.footstepDataList = stepData; return footsteps; }
private static Packet customConvertToIHMCMessage(FootstepDataListRosMessage message) throws Exception { FootstepDataListMessage footsteps = new FootstepDataListMessage(); footsteps.swingTime = message.getSwingTime(); footsteps.transferTime = message.getTransferTime(); footsteps.setUniqueId(message.getUniqueId()); footsteps.executionMode = ExecutionMode.values[message.getExecutionMode()]; ArrayList<FootstepDataMessage> stepData = new ArrayList<>(); for (FootstepDataRosMessage footstepDataRosMessage : message.getFootstepDataList()) { stepData.add((FootstepDataMessage) convertToIHMCMessage(footstepDataRosMessage)); } footsteps.footstepDataList = stepData; return footsteps; }
private static FrameInformation convertFrameInformationRosMessage(FrameInformationRosMessage message) { FrameInformation frameInformation = new FrameInformation(); frameInformation.setDataReferenceFrameId(message.getDataReferenceFrameId()); frameInformation.setTrajectoryReferenceFrameId(message.getTrajectoryReferenceFrameId()); return frameInformation; }
public void publish(Point2D[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX((float) points[i].getX()); point2d.setY((float) points[i].getY()); vertices.add(point2d); } publish(message); }
private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message) throws Exception { WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage(); wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId()); wholeBodyTrajectoryMessage.leftArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage()); wholeBodyTrajectoryMessage.rightArmTrajectoryMessage = (ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage()); wholeBodyTrajectoryMessage.leftHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage()); wholeBodyTrajectoryMessage.rightHandTrajectoryMessage = (HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage()); wholeBodyTrajectoryMessage.leftFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage()); wholeBodyTrajectoryMessage.rightFootTrajectoryMessage = (FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage()); wholeBodyTrajectoryMessage.chestTrajectoryMessage = (ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage()); wholeBodyTrajectoryMessage.pelvisTrajectoryMessage = (PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage()); return wholeBodyTrajectoryMessage; }
@Override public void onNewMessage(FootstepDataListRosMessage message) { ArrayList<us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage> footStepDataArrayList = new ArrayList<us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage>(); List<FootstepDataRosMessage> footStepDataList = message.getFootstepDataList(); FootStepDataMessageConverter.convertFootStepDataList(footStepDataList, footStepDataArrayList); ExecutionMode executionMode = ExecutionMode.values[(int) message.getExecutionMode()]; controllerCommunicator.send(new us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataListMessage(footStepDataArrayList,message.getSwingTime(),message.getTransferTime(),executionMode)); } }
public void publish(String messageType, long uid, long robotTimestamp, long lastReceivedTimestamp) { ihmc_msgs.LastReceivedMessage message = getMessage(); message.setType(messageType); message.setUniqueId(uid); message.setReceiveTimestamp(lastReceivedTimestamp); message.setTimeSinceLastReceived(robotTimestamp - lastReceivedTimestamp); publish(message); }
public void publish(Point2D32[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX(points[i].getX()); point2d.setY(points[i].getY()); vertices.add(point2d); } publish(message); } }
public static Point2dRosMessage convertPoint2d(Point2D point2d) { Point2dRosMessage point2dMessage = messageFactory.newFromType("ihmc_msgs/Point2dRosMessage"); if (point2d == null) { point2dMessage.setX(Double.NaN); point2dMessage.setY(Double.NaN); } else { point2dMessage.setX(point2d.getX()); point2dMessage.setY(point2d.getY()); } return point2dMessage; }
public void publish(Point2d[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX((float) points[i].getX()); point2d.setY((float) points[i].getY()); vertices.add(point2d); } publish(message); }
public static Point2dRosMessage convertPoint2d(Point2d point2d) { Point2dRosMessage point2dMessage = messageFactory.newFromType("ihmc_msgs/Point2dRosMessage"); if(point2d == null) { point2dMessage.setX(Double.NaN); point2dMessage.setY(Double.NaN); } else { point2dMessage.setX(point2d.getX()); point2dMessage.setY(point2d.getY()); } return point2dMessage; }
public void publish(Point2f[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX(points[i].getX()); point2d.setY(points[i].getY()); vertices.add(point2d); } publish(message); } }
public void publish(Point2f[] points, int numberOfVertices) { SupportPolygonRosMessage message = getMessage(); message.setNumberOfVertices(numberOfVertices); List<Point2dRosMessage> vertices = message.getVertices(); vertices.clear(); for (int i = 0; i < numberOfVertices; i++) { Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE); point2d.setX(points[i].getX()); point2d.setY(points[i].getY()); vertices.add(point2d); } publish(message); } }