myMidca.append_module(
"Plan",
planning.AsynchPyhopPlanner(methods.declare_methods,
operators.declare_ops))
myMidca.append_module("Act", act.AsynchronousAct())
return myMidca
thisDir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
MIDCA_ROOT = thisDir + "/../"
myMidca = ros_style_midca()
myMidca.logger.logOutput()
myMidca.mem.enableLogging(myMidca.logger)
rosMidca = rosrun.RosMidca(
myMidca,
incomingMsgHandlers=[
rosrun.FixedObjectLocationHandler(OBJ_LOC_TOPIC, "quad", myMidca),
rosrun.UtteranceHandler(UTTERANCE_TOPIC, myMidca),
rosrun.FeedbackHandler(rosrun.FEEDBACK_TOPIC, myMidca)
],
outgoingMsgHandlers=[
rosrun.OutgoingMsgHandler(asynch.POINT_TOPIC, String)
])
rosMidca.ros_connect()
rosMidca.run_midca()
MIDCA_ROOT = thisDir + "/../"
myMidca = ros_style_midca()
myMidca.logger.logOutput()
myMidca.mem.enableLogging(myMidca.logger)
# calibration
rosMidca = rosrun.RosMidca(myMidca, incomingMsgHandlers = [
#rosrun.CalibrationHandler("calibrate_done", myMidca),
rosrun.threeObjectsLocationHandler("obj_pos", myMidca),
rosrun.UtteranceHandler("cmds_received", myMidca),
rosrun.FeedbackHandler(rosrun.FEEDBACK_TOPIC, myMidca)],
outgoingMsgHandlers = [rosrun.OutgoingMsgHandler(asynch.LOC_TOPIC, String),
rosrun.OutgoingMsgHandler(asynch.GRAB_TOPIC, String),
rosrun.OutgoingMsgHandler(asynch.RELEASE_TOPIC, String),
rosrun.OutgoingMsgHandler(asynch.RAISE_TOPIC, String)])
rosMidca.ros_connect()
H = Calibrate.calibrate()
#Z = -0.15113003072395247
Z = -0.16113003072395247
myMidca.mem.set(myMidca.mem.CALIBRATION_MATRIX, H)
myMidca.mem.set(myMidca.mem.CALIBRATION_Z, Z)
#myMidca.mem.set(myMidca.mem.STACK_Z, 0.018148563732166244)
myMidca.mem.set(myMidca.mem.STACK_Z, -0.1097833784354784)