* SUBSCRIBER CALLBACKS - EB2008
* ---------------------------------------------------------------------- */
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
- static int last_response = 0;
+ struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
+ static int last_response_left = 0;
+ static int last_response_right = 0;
switch (info->status) {
case NEW_DATA:
// new data arrived and requested position equals actual position
- if (instance->flags == 0)
- robot.status[COMPONENT_VIDLE]=STATUS_OK;
+ if (instance->flags.left == 0 &&
+ instance->flags.right == 0)
+ robot.status[COMPONENT_JAWS] = STATUS_OK;
else
- robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
+ robot.status[COMPONENT_JAWS] = STATUS_WARNING;
- if (instance->response != last_response &&
- instance->response == act_vidle_get_last_reqest())
- FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
- last_response = instance->response;
+ if (instance->response.left != last_response_left &&
+ instance->response.right != last_response_right &&
+ instance->response.left == act_jaw_left_get_last_reqest() &&
+ instance->response.left == act_jaw_right_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+
+ last_response_left = instance->response.left;
+ last_response_right = instance->response.right;
break;
case DEADLINE:
- robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
+ robot.status[COMPONENT_JAWS] = STATUS_FAILED;
DBG("ORTE deadline occurred - actuator_status receive\n");
break;
}
}
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct lift_status_type *instance = (struct lift_status_type *)vinstance;
+ static int last_response = 0;
+ switch (info->status) {
+ case NEW_DATA:
+ // new data arrived and requested position equals actual position
+ if (instance->flags == 0)
+ robot.status[COMPONENT_LIFT] = STATUS_OK;
+ else
+ robot.status[COMPONENT_LIFT] = STATUS_WARNING;
+
+ if (instance->response != last_response &&
+ instance->response == act_lift_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+ last_response = instance->response;
+ break;
+ case DEADLINE:
+ robot.status[COMPONENT_LIFT] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - actuator_status receive\n");
+ break;
+ }
+}
+
void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
- robottype_publisher_vidle_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
/* create generic subscribers */
robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
//robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
- robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_cb, &robot.orte);
+ robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
+ robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
- robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
+ robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
return rv;
}