void DevicesModel::deviceUpdated(const QString& id, bool isVisible)
{
Q_UNUSED(isVisible);
int row = rowForDevice(id);
if (row < 0) {
// FIXME: when m_dbusInterface is not valid refreshDeviceList() does
// nothing and we can miss some devices.
// Someone can reproduce this problem by restarting kdeconnectd while
// kdeconnect's plasmoid is still running.
// Another reason for this branch is that we removed the device previously
// because of the filter settings.
qCDebug(KDECONNECT_INTERFACES) << "Adding missing or previously removed device" << id;
deviceAdded(id);
} else {
DeviceDbusInterface *dev = getDevice(row);
if (! passesFilter(dev)) {
beginRemoveRows(QModelIndex(), row, row);
delete m_deviceList.takeAt(row);
endRemoveRows();
qCDebug(KDECONNECT_INTERFACES) << "Removed changed device " << id;
} else {
const QModelIndex idx = index(row);
Q_EMIT dataChanged(idx, idx);
}
}
}
void ItemContainer::setTypeFilter(const std::list<std::string>& types)
{
mTypeFilter = types;
if (getSelectedItem() && !passesFilter(getSelectedItem()))
selectNone();
}
void ItemContainer::setEquipSlotsFilter(signed int slots)
{
mEquipSlotsFilter = slots;
if (getSelectedItem() && !passesFilter(getSelectedItem()))
selectNone();
}
void ICACHE_FLASH_ATTR mqttDataCb(uint32_t *args, const char* topic, uint32_t topic_len,
const char *data, uint32_t data_len) {
char *topicBuf = (char*) os_zalloc(topic_len + 1), *dataBuf = (char*) os_zalloc(data_len + 1);
char *tokens[10];
MQTT_Client* client = (MQTT_Client*) args;
os_memcpy(topicBuf, topic, topic_len);
topicBuf[topic_len] = 0;
os_memcpy(dataBuf, data, data_len);
dataBuf[data_len] = 0;
os_printf("Receive topic: %s, data: %s \r\n", topicBuf, dataBuf);
int tokenCount = splitString((char *) topicBuf, '/', tokens);
if (tokenCount > 0) {
if (strcmp("Raw", tokens[0]) == 0) {
if (tokenCount == 4 && strcmp(sysCfg.device_id, tokens[1]) == 0
&& strcmp("set", tokens[2]) == 0) {
if (strlen(dataBuf) < NAME_SIZE - 1) {
if (strcmp("name", tokens[3]) == 0) {
strcpy(sysCfg.deviceName, dataBuf);
} else if (strcmp("location", tokens[3]) == 0) {
strcpy(sysCfg.deviceLocation, dataBuf);
} else if (strcmp("updates", tokens[3]) == 0) {
sysCfg.updates = atoi(dataBuf);
os_timer_disarm(&transmit_timer);
os_timer_arm(&transmit_timer, sysCfg.updates * 1000, true);
}
publishDeviceInfo(client);
CFG_Save();
}
} else if (tokenCount == 5 && strcmp(sysCfg.device_id, tokens[1]) == 0
&& strcmp("set", tokens[3]) == 0 && strcmp("filter", tokens[4]) == 0) {
uint8 filterID = atoi(tokens[2]);
if (0 <= filterID && filterID <= 3) {
if (strlen(dataBuf) < (sizeof(sysCfg.filters[0]) - 2)) {
strcpy(sysCfg.filters[filterID], dataBuf);
CFG_Save();
}
}
}
} else if (strcmp("App", tokens[0]) == 0) {
if (tokenCount >= 2 && strcmp("date", tokens[1]) == 0) {
os_timer_disarm(&date_timer); // Restart it
os_timer_arm(&date_timer, 10 * 60 * 1000, false); //10 minutes
} else if (tokenCount == 5) {
if (passesFilter(&tokens[1])) {
saveData(tokens[1], tokens[2], tokens[3], tokens[4], dataBuf);
}
}
}
}
os_free(topicBuf);
os_free(dataBuf);
}
void ItemContainer::setTypeFilter(const std::string& type)
{
mTypeFilter.clear();
// "" is special-cased to remove the filter
if (!type.empty())
mTypeFilter.push_back(type);
if (getSelectedItem() && !passesFilter(getSelectedItem()))
selectNone();
}
void ItemContainer::draw(gcn::Graphics *graphics)
{
if (!isVisible())
return;
const int columns = std::max(1, getWidth() / gridWidth);
int gridSlot = 0; // The visible slot for drawing this item
for (int i = 0; i < mInventory->getSize(); i++)
{
Item *item = mInventory->getItem(i);
if (!item || item->getQuantity() <= 0 || !passesFilter(item))
continue;
// Work out the object's position,
const int itemX = (gridSlot % columns) * gridWidth;
const int itemY = (gridSlot / columns) * gridHeight;
// Draw selection image below selected item
if (mSelectedItemIndex == i)
static_cast<Graphics*>(graphics)->drawImage(mSelImg, itemX, itemY);
// Draw item icon
Image* image = item->getImage();
if (image)
static_cast<Graphics*>(graphics)->drawImage(image, itemX, itemY);
// Draw item caption
graphics->setFont(getFont());
graphics->setColor(guiPalette->getColor(item->isEquipped() ?
Palette::ITEM_EQUIPPED : Palette::TEXT));
const std::string& text =
item->isEquipped() ? STRING_EQUIPPED :
item->getQuantity() == 1 ? STRING_ONE :
toString(item->getQuantity());
graphics->drawText(
text,
itemX + gridWidth / 2, itemY + gridHeight - 11,
gcn::Graphics::CENTER);
// Move on to the next visible slot
gridSlot++;
}
// If there are no visible items, make sure nothing is selected;
// and inform the selection-listeners.
if (!gridSlot)
selectNone();
}
int ItemContainer::getVisibleSlot(const Item* searchItem) const
{
int itemCount = NO_ITEM;
for (int i = 0; i < mInventory->getSize(); i++)
{
Item *item = mInventory->getItem(i);
if (!item || item->getQuantity() <= 0 || !passesFilter(item))
continue;
itemCount++;
if (searchItem == item)
return itemCount;
}
return NO_ITEM;
}
Item* ItemContainer::getItemInVisibleSlot(const int gridSlot)
{
int itemCount = NO_ITEM;
for (int i = 0; i < mInventory->getSize(); i++)
{
Item *item = mInventory->getItem(i);
if (!item || item->getQuantity() <= 0 || !passesFilter(item))
continue;
itemCount++;
if (itemCount == gridSlot)
return item;
}
return NULL;
}
// Show ItemPopup
void ItemContainer::mouseMoved(gcn::MouseEvent &event)
{
if (!mShowItemInfo || mPopupMenu->isVisible())
return;
Item *item = getItem(event.getX(), event.getY());
if (item && passesFilter(item))
{
if (item->getInfo().getName() != mItemPopup->getItemName())
mItemPopup->setItem(item->getInfo());
mItemPopup->updateColors();
mItemPopup->view(gui->getMouseX(), gui->getMouseY());
}
else
mItemPopup->setVisible(false);
}
void DevicesModel::deviceAdded(const QString& id)
{
if (rowForDevice(id) >= 0) {
Q_ASSERT_X(false, "deviceAdded", "Trying to add a device twice");
return;
}
DeviceDbusInterface* dev = new DeviceDbusInterface(id, this);
Q_ASSERT(dev->isValid());
if (! passesFilter(dev)) {
delete dev;
return;
}
beginInsertRows(QModelIndex(), m_deviceList.size(), m_deviceList.size());
appendDevice(dev);
endInsertRows();
}
void ItemContainer::recalculateHeight()
{
int numOfItems = 0;
for (int i = 0; i < mInventory->getSize(); i++)
{
Item *item = mInventory->getItem(i);
if (!item || item->getQuantity() <= 0 || !passesFilter(item))
continue;
numOfItems++;
}
const int cols = std::max(1, getWidth() / gridWidth);
const int rows = (numOfItems / cols) + (numOfItems % cols > 0 ? 1 : 0);
const int height = rows * gridHeight + 8;
if (height != getHeight())
setHeight(height);
}
void ItemContainer::showItemPopup(bool show)
{
Item *item = getSelectedItem();
if (!item || item->getQuantity() <= 0 || !passesFilter(item))
{
mItemPopup->setVisible(false);
return;
}
if (item->getInfo().getName() != mItemPopup->getItemName())
mItemPopup->setItem(item->getInfo());
int x = 0;
int y = 0;
getPopupLocation(false, x, y);
mItemPopup->updateColors();
mItemPopup->view(x, y);
mItemPopup->setVisible(mShowItemInfo ? show : false);
}
bool WIZARD_FPLIB_TABLE::checkFiles() const
{
// Get current selection (files & directories)
wxArrayString candidates;
m_filePicker->GetPaths( candidates );
// Workaround, when you change filters "/" is automatically selected
int slash_index = candidates.Index( "/", true, true );
if( slash_index != wxNOT_FOUND )
candidates.RemoveAt( slash_index, 1 );
if( candidates.IsEmpty() )
return false;
// Verify all the files/folders comply to the selected library type filter
for( unsigned int i = 0; i < candidates.GetCount(); ++i )
{
if( !passesFilter( candidates[i], m_filePicker->GetFilterIndex() ) )
return false;
}
return true;
}
void ItemContainer::setSelectedItemIndex(int index)
{
int newSelectedItemIndex;
Item* item = mInventory->getItem(index);
if (item && passesFilter(item))
newSelectedItemIndex = index;
else
newSelectedItemIndex = NO_ITEM;
if (mSelectedItemIndex != newSelectedItemIndex)
{
mSelectedItemIndex = newSelectedItemIndex;
if (mSelectedItemIndex == NO_ITEM)
mLastSelectedItemId = NO_ITEM;
else
mLastSelectedItemId = item->getId();
gcn::Rectangle scroll;
const int columns = std::max(1, getWidth() / gridWidth);
const int itemY = getVisibleSlot(item) / columns;
if (mSelectedItemIndex == NO_ITEM)
scroll.y = 0;
else
scroll.y = gridHeight * itemY;
scroll.height = gridHeight;
showPart(scroll);
showItemPopup(mShowItemInfo);
distributeValueChangedEvent();
}
}
static struct annoRow *aggvGenRows( struct annoGratorGpVar *self, struct variant *variant,
struct genePred *pred, struct annoRow *inRow,
struct lm *callerLm)
// put out annoRows for all the gpFx that arise from variant and pred
{
struct dnaSeq *transcriptSequence = genePredToGenomicSequence(pred, self->curChromSeq->dna,
self->lm);
struct gpFx *effects = gpFxPredEffect(variant, pred, transcriptSequence, self->lm);
struct annoRow *rows = NULL;
for(; effects; effects = effects->next)
{
// Intergenic variants never pass through here so we don't have to filter them out
// here if self->funcFilter is null.
if (self->funcFilter == NULL || passesFilter(self, effects))
{
struct annoRow *row = aggvEffectToRow(self, effects, inRow, callerLm);
slAddHead(&rows, row);
}
}
slReverse(&rows);
return rows;
}
void executeCB(const segbot_arm_manipulation::TabletopGraspGoalConstPtr &goal)
{
if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::GRASP){
if (goal->cloud_clusters.size() == 0){
ROS_INFO("[segbot_tabletop_grap_as.cpp] No object point clouds received...aborting");
as_.setAborted(result_);
return;
}
ROS_INFO("[segbot_tabletop_grap_as.cpp] Received action request...proceeding.");
listenForArmData(40.0);
//the result
segbot_arm_manipulation::TabletopGraspResult result;
//get the data out of the goal
Eigen::Vector4f plane_coef_vector;
for (int i = 0; i < 4; i ++)
plane_coef_vector(i)=goal->cloud_plane_coef[i];
int selected_object = goal->target_object_cluster_index;
PointCloudT::Ptr target_object (new PointCloudT);
pcl::PCLPointCloud2 target_object_pc2;
pcl_conversions::toPCL(goal->cloud_clusters.at(goal->target_object_cluster_index),target_object_pc2);
pcl::fromPCLPointCloud2(target_object_pc2,*target_object);
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Publishing point cloud...");
cloud_grasp_pub.publish(goal->cloud_clusters.at(goal->target_object_cluster_index));
//wait for response at 5 Hz
listenForGrasps(40.0);
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Heard %i grasps",(int)current_grasps.grasps.size());
//next, compute approach and grasp poses for each detected grasp
//wait for transform from visual space to arm space
std::string sensor_frame_id = goal->cloud_clusters.at(goal->target_object_cluster_index).header.frame_id;
listener.waitForTransform(sensor_frame_id, "mico_link_base", ros::Time(0), ros::Duration(3.0));
//here, we'll store all grasp options that pass the filters
std::vector<GraspCartesianCommand> grasp_commands;
for (unsigned int i = 0; i < current_grasps.grasps.size(); i++){
GraspCartesianCommand gc_i = segbot_arm_manipulation::grasp_utils::constructGraspCommand(current_grasps.grasps.at(i),HAND_OFFSET_APPROACH,HAND_OFFSET_GRASP, sensor_frame_id);
//filter 1: if the grasp is too close to plane, reject it
bool ok_with_plane = segbot_arm_manipulation::grasp_utils::checkPlaneConflict(gc_i,plane_coef_vector,MIN_DISTANCE_TO_PLANE);
//for filter 2, the grasps need to be in the arm's frame of reference
listener.transformPose("mico_link_base", gc_i.approach_pose, gc_i.approach_pose);
listener.transformPose("mico_link_base", gc_i.grasp_pose, gc_i.grasp_pose);
//filter 2: apply grasp filter method in request
bool passed_filter = passesFilter(goal->grasp_filter_method,gc_i);
if (passed_filter && ok_with_plane){
ROS_INFO("Found grasp fine with filter and plane");
//filter two -- if IK fails
moveit_msgs::GetPositionIK::Response ik_response_approach = segbot_arm_manipulation::computeIK(nh_,gc_i.approach_pose);
if (ik_response_approach.error_code.val == 1){
moveit_msgs::GetPositionIK::Response ik_response_grasp = segbot_arm_manipulation::computeIK(nh_,gc_i.grasp_pose);
if (ik_response_grasp.error_code.val == 1){
ROS_INFO("...grasp fine with IK");
//now check to see how close the two sets of joint angles are -- if the joint configurations for the approach and grasp poses differ by too much, the grasp will not be accepted
std::vector<double> D = segbot_arm_manipulation::getJointAngleDifferences(ik_response_approach.solution.joint_state, ik_response_grasp.solution.joint_state);
double sum_d = 0;
for (int p = 0; p < D.size(); p++){
sum_d += D[p];
}
if (sum_d < ANGULAR_DIFF_THRESHOLD){
//ROS_INFO("Angle diffs for grasp %i: %f, %f, %f, %f, %f, %f",(int)grasp_commands.size(),D[0],D[1],D[2],D[3],D[4],D[5]);
//ROS_INFO("Sum diff: %f",sum_d);
//store the IK results
gc_i.approach_q = ik_response_approach.solution.joint_state;
gc_i.grasp_q = ik_response_grasp.solution.joint_state;
grasp_commands.push_back(gc_i);
ROS_INFO("...fine with continuity");
}
}
}
}
}
//check to see if all potential grasps have been filtered out
if (grasp_commands.size() == 0){
ROS_WARN("[segbot_tabletop_grasp_as.cpp] No feasible grasps found. Aborting.");
as_.setAborted(result_);
return;
}
//make sure we're working with the correct tool pose
listenForArmData(30.0);
int selected_grasp_index = -1;
if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_ORIENTATION_SELECTION){
//find the grasp with closest orientatino to current pose
double min_diff = 1000000.0;
for (unsigned int i = 0; i < grasp_commands.size(); i++){
double d_i = segbot_arm_manipulation::grasp_utils::quat_angular_difference(grasp_commands.at(i).approach_pose.pose.orientation, current_pose.pose.orientation);
ROS_INFO("Distance for pose %i:\t%f",(int)i,d_i);
if (d_i < min_diff){
selected_grasp_index = (int)i;
min_diff = d_i;
}
}
}
else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::RANDOM_SELECTION){
srand (time(NULL));
selected_grasp_index = rand() % grasp_commands.size();
ROS_INFO("Randomly selected grasp = %i",selected_grasp_index);
}
else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_JOINTSPACE_SELECTION){
double min_diff = 1000000.0;
for (unsigned int i = 0; i < grasp_commands.size(); i++){
std::vector<double> D_i = segbot_arm_manipulation::getJointAngleDifferences(grasp_commands.at(i).approach_q, current_state);
double sum_d = 0;
for (int p = 0; p < D_i.size(); p++)
sum_d += D_i[p];
if (sum_d < min_diff){
selected_grasp_index = (int)i;
min_diff = sum_d;
}
}
}
if (selected_grasp_index == -1){
ROS_WARN("[segbot_tabletop_grasp_as.cpp] Grasp selection failed. Aborting.");
as_.setAborted(result_);
return;
}
//compute RPY for target pose
ROS_INFO("Selected approach pose:");
ROS_INFO_STREAM(grasp_commands.at(selected_grasp_index).approach_pose);
//publish individual pose for visualization purposes
pose_pub.publish(grasp_commands.at(selected_grasp_index).approach_pose);
//close fingers while moving
segbot_arm_manipulation::closeHand();
//move to approach pose -- do it twice to correct
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
//open fingers
segbot_arm_manipulation::openHand();
//move to grasp pose
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).grasp_pose);
//close hand
segbot_arm_manipulation::closeHand();
result_.success = true;
as_.setSucceeded(result_);
return;
}
else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER){
//TO DO: move to handover position
ROS_INFO("Starting handover action...");
//listen for haptic feedback
double rate = 40;
ros::Rate r(rate);
double total_grav_free_effort = 0;
double total_delta;
double delta_effort[6];
listenForArmData(40.0);
sensor_msgs::JointState prev_effort_state = current_effort;
double elapsed_time = 0;
while (ros::ok()){
ros::spinOnce();
total_delta=0.0;
for (int i = 0; i < 6; i ++){
delta_effort[i] = fabs(current_effort.effort[i]-prev_effort_state.effort[i]);
total_delta+=delta_effort[i];
ROS_INFO("Total delta=%f",total_delta);
}
if (total_delta > fabs(FORCE_HANDOVER_THRESHOLD)){
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Force detected");
//now open the hand
segbot_arm_manipulation::openHand();
result_.success = true;
as_.setSucceeded(result_);
return;
}
r.sleep();
elapsed_time+=(1.0)/rate;
if (goal->timeout_seconds > 0 && elapsed_time > goal->timeout_seconds){
ROS_WARN("Handover action timed out...");
result_.success = false;
as_.setAborted(result_);
return;
}
}
result_.success = true;
as_.setSucceeded(result_);
}
else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER_FROM_HUMAN){
//open fingers
segbot_arm_manipulation::openHand();
//update readings
listenForArmData(40.0);
bool result = waitForForce(FORCE_HANDOVER_THRESHOLD,goal->timeout_seconds);
if (result){
segbot_arm_manipulation::closeHand();
result_.success = true;
as_.setSucceeded(result_);
}
else {
result_.success = false;
as_.setAborted(result_);
}
}
}
