bundles/examples/org.eclipse.apogy.examples.robotic_arm.ros/src/org/eclipse/apogy/examples/robotic_arm/ros/listeners/RoboticArmTelemetryListener.java

/*******************************************************************************
 * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
<<<<<<< HEAD
 * Contributors:
 *     Pierre Allard - initial API and implementation
 *     Regent L'Archeveque
 * 
=======
 * Contributors:  
 *     Pierre Allard,
 *     Regent L'Archeveque  - initial API and implementation
 *
>>>>>>> refs/heads/eclipse_pa
 * SPDX-License-Identifier: EPL-1.0
 *******************************************************************************/
package org.eclipse.apogy.examples.robotic_arm.ros.listeners;

import org.eclipse.apogy.examples.robotic_arm.MoveSpeedLevel;
import org.eclipse.apogy.examples.robotic_arm.RoboticArm;
import org.eclipse.apogy.examples.robotic_arm.ros.RoboticArmTelemetry;
import org.ros.message.MessageListener;

public class RoboticArmTelemetryListener
		implements MessageListener<org.eclipse.apogy.examples.robotic_arm.ros.RoboticArmTelemetry> {
	private RoboticArm roboticArm = null;

	public RoboticArmTelemetryListener(RoboticArm roboticArm) {
		setRoboticArm(roboticArm);
	}

	public void setRoboticArm(RoboticArm roboticArm) {
		this.roboticArm = roboticArm;
	}

	@Override
	public void onNewMessage(RoboticArmTelemetry msg) {
		try {
			if (this.roboticArm != null) {
				// Updates the joint angles. Note that here that the ROS telemetry is defined in
				// radians, but our model is in degrees.
				this.roboticArm.setTurretAngle(Math.toDegrees(msg.getTurretAngle()));
				this.roboticArm.setShoulderAngle(Math.toDegrees(msg.getShoulderAngle()));
				this.roboticArm.setElbowAngle(Math.toDegrees(msg.getElbowAngle()));
				this.roboticArm.setWristAngle(Math.toDegrees(msg.getWristAngle()));

				// Updates the moving flag.
				this.roboticArm.setArmMoving(msg.getMoving());

				// Updates the MoveSpeed value.
				this.roboticArm.setSpeed(MoveSpeedLevel.get(msg.getSpeed().getSpeedStatus()));
			}
		} catch (Exception e) {

		}
	}

}