package com.roboclub.robobuggy.nodes.planners;
import com.roboclub.robobuggy.messages.BrakeControlMessage;
import com.roboclub.robobuggy.messages.DriveControlMessage;
import com.roboclub.robobuggy.messages.GPSPoseMessage;
import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode;
import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode;
import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode;
import com.roboclub.robobuggy.ros.Message;
import com.roboclub.robobuggy.ros.MessageListener;
import com.roboclub.robobuggy.ros.NodeChannel;
import com.roboclub.robobuggy.ros.Publisher;
import com.roboclub.robobuggy.ros.Subscriber;
import java.util.Date;
/**
* Abstract class used to represent all {@link BuggyNode}s used as path
* planners.
*
* @author Zachary Dawson
*/
public abstract class PathPlannerNode extends BuggyDecoratorNode {
private Publisher steeringCommandPub;
private Publisher brakingCommandPub;
/**
* Construct a new {@link PathPlannerNode}
*
* @param channel {@link NodeChannel} on which to broadcast status
* information about the node
*/
public PathPlannerNode(NodeChannel channel) {
super(new BuggyBaseNode(channel), "pathPlannerNode");
steeringCommandPub = new Publisher(NodeChannel.DRIVE_CTRL.getMsgPath());
brakingCommandPub = new Publisher(NodeChannel.BRAKE_CTRL.getMsgPath());
}
/**
* {@inheritDoc}
*/
@Override
protected final boolean startDecoratorNode() {
//Initialize subscribers to pose estimations
new Subscriber("pathPlanner", NodeChannel.POSE.getMsgPath(), new MessageListener() {
@Override
public void actionPerformed(String topicName, Message m) {
updatePositionEstimate((GPSPoseMessage) m);
steeringCommandPub.publish(new DriveControlMessage(new Date(),
getCommandedSteeringAngle()));
brakingCommandPub.publish(new BrakeControlMessage(new Date(),
getDeployBrakeValue()));
}
});
return true;
}
/**
* {@inheritDoc}
*/
@Override
protected boolean shutdownDecoratorNode() {
//Always shuts down properly
return true;
}
/**
* Called whenever there is an update to the robots pose. This method
* updates the planner's notion of the robot pose, causing a re-planning
* of the path if needed.
*
* @param m {@link PoseMessage} containing the new pose of the robot
*/
protected abstract void updatePositionEstimate(GPSPoseMessage m);
/**
* Returns the steering angle to which the {@link PathPlanner} thinks the
* buggy's steering should be commanded to follow the desired path.
*
* @return desired commanded steering angle
*/
protected abstract double getCommandedSteeringAngle();
/**
* Returns the brake value to which the {@link PathPlanner} thinks the
* buggy's brakes should be commanded.
*
* @return desired commanded brake value (true is deployed)
*/
protected abstract boolean getDeployBrakeValue();
}