/* ////////////////////////////////////////////////////////////////////////////
* Update the video display
*/
void TTMpeg2MainWnd::updateDisplay(int indexPos)
{
sliderUpdateFrame = false;
currentFrame->showFrameAt(indexPos);
if (indexPos == 0)
currentFrame->moveToFirstFrame(true);
scroller->setValue(indexPos);
updateFrameInfo(indexPos);
updateVideoFileInfo(mpegStream);
}
/* ////////////////////////////////////////////////////////////////////////////
* Slider value changed
*/
void TTMpeg2MainWnd::onSliderValueChanged(int indexPos)
{
if (!isProjectOpen)
return;
if (sliderUpdateFrame)
{
int newFramePos = mpegStream->moveToIndexPos(indexPos, 0);
currentFrame->showFrameAt(newFramePos);
updateFrameInfo(indexPos);
updateVideoFileInfo(mpegStream);
}
sliderUpdateFrame = true;
}
void AnimatedSprite::updateAnim() {
_frameChanged = false;
if (_newAnimFileHash == 0) {
if (_newStickFrameIndex != -1) {
_currStickFrameIndex = _newStickFrameIndex == STICK_LAST_FRAME ? _animResource.getFrameCount() - 1 : _newStickFrameIndex;
_newStickFrameIndex = -1;
} else if (_newStickFrameHash != 0) {
_currStickFrameIndex = MAX<int16>(0, _animResource.getFrameIndex(_newStickFrameHash));
_newStickFrameHash = 0;
}
if (_newAnimFileHash == 0 && _currFrameIndex != _currStickFrameIndex) {
if (_currFrameTicks != 0 && (--_currFrameTicks == 0) && _animResource.getFrameCount() != 0) {
if (_nextAnimFileHash != 0) {
if (_animResource.load(_nextAnimFileHash)) {
_currAnimFileHash = _nextAnimFileHash;
} else {
_animResource.load(calcHash("sqDefault"));
_currAnimFileHash = 0;
}
if (_replOldColor != _replNewColor) {
_animResource.setRepl(_replOldColor, _replNewColor);
}
_nextAnimFileHash = 0;
if (_animStatus != 0) {
_currFrameIndex = _plFirstFrameHash != 0 ? MAX<int16>(0, _animResource.getFrameIndex(_plFirstFrameHash)) : 0;
_lastFrameIndex = _plLastFrameHash != 0 ? MAX<int16>(0, _animResource.getFrameIndex(_plLastFrameHash)) : _animResource.getFrameCount() - 1;
} else {
_currFrameIndex = _plFirstFrameIndex != -1 ? _plFirstFrameIndex : _animResource.getFrameCount() - 1;
_lastFrameIndex = _plLastFrameIndex != -1 ? _plLastFrameIndex : _animResource.getFrameCount() - 1;
}
} else {
updateFrameIndex();
}
if (_newAnimFileHash == 0)
updateFrameInfo();
}
}
}
if (_newAnimFileHash != 0) {
if (_animStatus == 2) {
_currStickFrameIndex = _currFrameIndex;
} else {
if (_animStatus == 1) {
if (_animResource.load(_newAnimFileHash)) {
_currAnimFileHash = _newAnimFileHash;
} else {
_animResource.load(calcHash("sqDefault"));
_currAnimFileHash = 0;
}
if (_replOldColor != _replNewColor) {
_animResource.setRepl(_replOldColor, _replNewColor);
}
_newAnimFileHash = 0;
_currFrameIndex = _plFirstFrameHash != 0 ? MAX<int16>(0, _animResource.getFrameIndex(_plFirstFrameHash)) : 0;
_lastFrameIndex = _plLastFrameHash != 0 ? MAX<int16>(0, _animResource.getFrameIndex(_plLastFrameHash)) : _animResource.getFrameCount() - 1;
} else {
if (_animResource.load(_newAnimFileHash)) {
_currAnimFileHash = _newAnimFileHash;
} else {
_animResource.load(calcHash("sqDefault"));
_currAnimFileHash = 0;
}
if (_replOldColor != _replNewColor) {
_animResource.setRepl(_replOldColor, _replNewColor);
}
_newAnimFileHash = 0;
_currFrameIndex = _plFirstFrameIndex != -1 ? _plFirstFrameIndex : _animResource.getFrameCount() - 1;
_lastFrameIndex = _plLastFrameIndex != -1 ? _plLastFrameIndex : _animResource.getFrameCount() - 1;
}
updateFrameInfo();
}
if (_newStickFrameIndex != -1) {
_currStickFrameIndex = _newStickFrameIndex == STICK_LAST_FRAME ? _animResource.getFrameCount() - 1 : _newStickFrameIndex;
_newStickFrameIndex = -1;
} else if (_newStickFrameHash != 0) {
_currStickFrameIndex = MAX<int16>(0, _animResource.getFrameIndex(_newStickFrameHash));
_newStickFrameHash = 0;
}
}
}
void QNode::clingoInterfaceHandler(
const segbot_logical_translator_gui::ClingoInterfaceGoalConstPtr &req) {
segbot_logical_translator_gui::ClingoInterfaceResult resp;
if (req->command.op == "approach" || req->command.op == "gothrough") {
std::string door_name = req->command.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
if (door_idx == (size_t)-1) {
// Interface failure
resp.success = false;
resp.status = "Could not resolve argument: " + door_name;
} else {
bwi_mapper::Point2f approach_pt;
float approach_yaw = 0;
bool door_approachable = false;
if (req->command.op == "approach") {
door_approachable = handler_->getApproachPoint(door_idx,
bwi_mapper::Point2f(robot_x_, robot_y_), approach_pt,
approach_yaw);
} else {
door_approachable = handler_->getThroughDoorPoint(door_idx,
bwi_mapper::Point2f(robot_x_, robot_y_), approach_pt,
approach_yaw);
}
if (door_approachable) {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = req->command.op + " " + door_name;
Q_EMIT updateFrameInfo();
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "/map"; //TODO
pose.pose.position.x = approach_pt.x;
pose.pose.position.y = approach_pt.y;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(approach_yaw),
pose.pose.orientation);
float start_time = ros::Time::now().toSec();
bool success = executeRobotGoal(pose);
float cost = ros::Time::now().toSec() - start_time;
resp.success = success;
// Publish the observable fluents
senseDoorProximity(resp.observable_fluents, door_idx);
if (success) {
if (prev_door_idx_ != -1 &&
prev_door_idx_ != door_idx) {
size_t location_idx =
handler_->getLocationIdx(bwi_mapper::Point2f(robot_x_, robot_y_));
std::string at_str = handler_->getLocationString(location_idx);
ce_->addSample(at_str, prev_door_idx_, door_idx, cost);
}
prev_door_idx_ = door_idx;
} else {
prev_door_idx_ = -1;
}
// Close a door is opened previously
if (success) {
if (sim_auto_door_ && close_door_idx_ != -1) {
gh_->closeDoor(close_door_idx_);
close_door_idx_ = -1;
}
}
// We can't sense the door once we go through as we won't be facing
// it. We should only sense a door when we approach it.
if (req->command.op == "approach") {
segbot_logical_translator_gui::ClingoFluent door_open;
door_open.args.push_back(door_name);
if (gh_->isDoorOpen(door_idx)) {
door_open.op = "open";
resp.observable_fluents.push_back(door_open);
} else {
door_open.op = "-open";
resp.observable_fluents.push_back(door_open);
}
}
} else {
// Planning failure
resp.success = false;
resp.status = "Cannot approach " + door_name + " from here.";
}
}
} else if (req->command.op == "opendoor") {
std::string door_name = req->command.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
if (door_idx == (size_t)-1) {
resp.success = false;
resp.status = "Could not resolve argument: " + door_name;
} else {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = "Can you please open " + door_name;
Q_EMIT updateFrameInfo();
/* Wait for the door to be opened */
ros::Rate r(1);
int count = 0;
bool door_open = false;
while (!door_open && count < 30) {
if (sim_auto_door_ && count == 0) {
gh_->openDoor(door_idx);
close_door_idx_ = door_idx;
}
if (as_->isPreemptRequested() || !ros::ok()) { // TODO What about goal not being active or new goal?
ROS_INFO("Preempting action");
as_->setPreempted();
return;
}
door_open = gh_->isDoorOpen(door_idx);
count++;
r.sleep();
}
/* Update the display and send back appropriate fluents */
segbot_logical_translator_gui::ClingoFluent door_open_fluent;
door_open_fluent.args.push_back(door_name);
if (!door_open) {
display_text_ = "Oh no! The request timed out. Let me think...";
Q_EMIT updateFrameInfo();
door_open_fluent.op = "-open";
resp.success = false;
resp.status = "User unable to open door";
} else {
display_text_ = "Thank you for opening the door!";
Q_EMIT updateFrameInfo();
door_open_fluent.op = "open";
resp.success = true;
}
resp.observable_fluents.push_back(door_open_fluent);
}
} else if (req->command.op == "sense") {
std::string sense_fluent_op = req->sense_fluent.op;
if (sense_fluent_op == "open") {
std::string door_name = req->sense_fluent.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
segbot_logical_translator_gui::ClingoFluent door_open;
door_open.args.push_back(door_name);
if (gh_->isDoorOpen(door_idx)) {
door_open.op = "open";
resp.observable_fluents.push_back(door_open);
} else {
door_open.op = "-open";
resp.observable_fluents.push_back(door_open);
}
resp.success = true;
} else if (sense_fluent_op == "beside" || sense_fluent_op == "facing") {
std::string door_name = req->sense_fluent.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
bwi_mapper::Point2f robot_loc(robot_x_, robot_y_);
bool op_door = (sense_fluent_op == "beside") ?
handler_->isRobotBesideDoor(robot_loc, robot_yaw_, 1.5, door_idx) :
handler_->isRobotFacingDoor(robot_loc, robot_yaw_, 1.5, door_idx);
if (!op_door) {
segbot_logical_translator_gui::ClingoFluent n_op;
n_op.op = "-" + sense_fluent_op;
n_op.args.push_back(door_name);
resp.observable_fluents.push_back(n_op);
} else {
segbot_logical_translator_gui::ClingoFluent op;
op.op = sense_fluent_op;
op.args.push_back(door_name);
resp.observable_fluents.push_back(op);
}
resp.success = true;
} else if (sense_fluent_op == "ploc") {
person_name_ = req->sense_fluent.args[0];
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = true;
display_text_ = "Can you tell me where " + person_name_ + " is?";
location_received_ = false;
Q_EMIT updateFrameInfo();
/* Wait for the location to be received */
ros::Rate r(10);
int count = 0;
bool door_open = false;
while (!location_received_ && count < 300) {
if (as_->isPreemptRequested() || !ros::ok()) { // TODO What about goal not being active or new goal?
ROS_INFO("Preempting action");
as_->setPreempted();
return;
}
count++;
r.sleep();
}
if (location_received_) {
display_text_ = "Thank you! I will try and find " + person_name_ + " in " + person_location_;
segbot_logical_translator_gui::ClingoFluent person_loc_fluent;
person_loc_fluent.op = "inside";
person_loc_fluent.args.push_back(person_name_);
person_loc_fluent.args.push_back(person_location_);
resp.observable_fluents.push_back(person_loc_fluent);
} else {
display_text_ = "Oh no! The request timed out. Let me think...";
}
location_box_enabled_ = false;
Q_EMIT updateFrameInfo();
}
} else if (req->command.op == "evaluate") {
resp.plan_cost = 0;
resp.success = true;
bwi_mapper::Point2f prev_pt(robot_x_, robot_y_), approach_pt;
float prev_yaw = robot_yaw_, approach_yaw = 0;
for (size_t i = 0; i < req->evaluate_fluents.size(); ++i) {
if (req->evaluate_fluents[i].op == "approach") {
std::string door_name = req->evaluate_fluents[i].args[0];
std::cout << "door_name: " << door_name << std::endl;
size_t door_idx = handler_->getDoorIdx(door_name);
bool door_approachable = handler_->getApproachPoint(door_idx,
prev_pt, approach_pt, approach_yaw);
if (!door_approachable) {
resp.plan_cost = std::numeric_limits<float>::max();
resp.success = false;
resp.status = "unable to evaluate one approach leg of plan.";
break;
}
resp.plan_cost += handler_->getPathCost(prev_pt, prev_yaw, approach_pt, approach_yaw);
} else if (req->evaluate_fluents[i].op == "gothrough") {
std::string door_name = req->evaluate_fluents[i].args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
bool door_approachable = handler_->getThroughDoorPoint(door_idx,
prev_pt, approach_pt, approach_yaw);
if (!door_approachable) {
resp.plan_cost = std::numeric_limits<float>::max();
resp.success = false;
resp.status = "unable to evaluate one gothrough leg of plan.";
std::cout << "from " << prev_pt << " to " << approach_pt << std::endl;
break;
}
resp.plan_cost +=
bwi_mapper::getMagnitude(approach_pt - prev_pt); // don't worry about closed doors for now
} else {
continue;
}
prev_pt = approach_pt;
prev_yaw = approach_yaw;
}
} else if (req->command.op == "greet") {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = "Hello " + req->command.args[0] + "!!";
Q_EMIT updateFrameInfo();
segbot_logical_translator_gui::ClingoFluent visiting;
visiting.op = "visiting";
visiting.args.push_back(req->command.args[0]);
resp.observable_fluents.push_back(visiting);
resp.success = true;
} else if (req->command.op == "noop") {
resp.success = true;
senseDoorProximity(resp.observable_fluents);
} else if (req->command.op == "finish") {
ROS_INFO("RECEIVED FINISH");
resp.success = true;
prev_door_idx_ = -1; // Robot may be teleported
ce_->finalizeEpisode();
}
// Get location
size_t location_idx =
handler_->getLocationIdx(bwi_mapper::Point2f(robot_x_, robot_y_));
if (location_idx == (size_t) -1) {
ROS_ERROR("Unable to compute position");
} else {
std::string at_str = handler_->getLocationString(location_idx);
segbot_logical_translator_gui::ClingoFluent at;
at.op = "at";
at.args.push_back(at_str);
resp.observable_fluents.push_back(at);
}
/* boost::this_thread::sleep(boost::posix_time::seconds(1)); */
if (resp.success) {
as_->setSucceeded(resp, resp.status);
} else {
as_->setAborted(resp, resp.status);
}
}
