bool detection::GraspPointDetector::pick(moveit::planning_interface::MoveGroup &group, const std::string &object, moveit_msgs::Grasp &grasp, bool plan_only) { if (!pick_action_client_) { ROS_ERROR_STREAM("Pick action client not found"); return false; } if (!pick_action_client_->isServerConnected()) { ROS_ERROR_STREAM("Pick action server not connected"); return false; } moveit_msgs::PickupGoal goal; constructGoal(group, goal, object, PLANNER_NAME_, plan_only); goal.possible_grasps.push_back(grasp); if (plan_only) { pick_action_client_->cancelAllGoals(); pick_action_client_->sendGoal(goal); if (!pick_action_client_->waitForResult(ros::Duration(5.0))) ROS_DEBUG_STREAM("Pickup action returned early"); bool plan_succeeded = pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED; ROS_DEBUG_STREAM_COND(!plan_succeeded, "Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText()); } else { try { pickMovement(pick_action_client_, goal, ros::Duration(25.0)); } catch (ComputeFailedException ex) { ROS_INFO_NAMED(DETECTION(), "%s", ex.what()); return false; } } return true; }
bool Output::draw(cv::Point2f ui_offset) { boost::timer t1; bool is_valid = false; bool is_dirty = false; const bool window_decorations_on = getSignal("decor", is_valid, is_dirty); if (is_dirty) { setSignal("decor", window_decorations_on); } cv_bridge::CvImage cv_image; cv_image.header.stamp = ros::Time::now(); cv_image.image = getImage("in"); cv_image.encoding = "rgba8"; sensor_msgs::ImagePtr msg = cv_image.toImageMsg(); /* sensor_msgs::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); camera_info->height = cv_image.image.cols; camera_info->width = cv_image.image.rows; camera_info->header.stamp = cv_image.header.stamp; camera_info->header.seq = seq_++; */ pub_.publish(msg); //, camera_info); ROS_DEBUG_STREAM_COND(log_level > 4, "decor draw time" << t1.elapsed()); #if 0 XWindowAttributes xwAttr, xwAttr2; // these don't seem to be updating x and y // Status ret = XGetWindowAttributes( display, win, &xwAttr ); /* Occasionally getting this error if decorate/undecorate multiple times. I could set own XSetErrorHandler(), and there are suggestions to XSync or X Error of failed request: BadWindow (invalid Window parameter) Major opcode of failed request: 3 (X_GetWindowAttributes) Resource id in failed request: 0x14030e7 Serial number of failed request: 3334 Current serial number in output stream: 3335 [Thread 0x7ffff7f9b7c0 (LWP 4483) exited] [Inferior 1 (process 4483) exited with code 01] * */ // this has the real x and y position, // but does turning off decorations cause this to change? ROS_DEBUG_STREAM_COND(log_level > 2, name << " 1 top " << toplevel_parent); Status ret = XGetWindowAttributes(display, toplevel_parent, &xwAttr); if (ret == 0) { ROS_ERROR_STREAM("bad xgetwindowattributes"); return false; } if (win == toplevel_parent) { xwAttr2 = xwAttr; } else { // this holds the 'true' width and height (the inner window not including // gnome decor?) the x and y are relative to the toplevel_parent ROS_DEBUG_STREAM_COND(log_level > 2, name << " 2 win " << win); Status ret2 = XGetWindowAttributes(display, win, &xwAttr2); if (ret2 == 0) { ROS_ERROR_STREAM("bad xgetwindowattributes"); return false; } } ROS_DEBUG_STREAM_COND(log_level > 6, name << " top " << xwAttr.x << " " << xwAttr.y << ", " << xwAttr.width << " " << xwAttr.height << " win " << xwAttr2.x << " " << xwAttr2.y << ", " << xwAttr2.width << " " << xwAttr2.height); // TBD the user ought to be able to force x,y,w,h changes // the logic would be that if the window attributes disagree with the last // update, use those, if the getSignal value is different than the old signal, // that means the user wants the window resized int wm_x = xwAttr.x; // + xwAttr2.x; int wm_y = xwAttr.y; // + xwAttr2.y; if ( (x != wm_x) || (y != wm_y) || (w != xwAttr2.width) || (h != xwAttr2.height) ) { // don't do anything, the window is already set properly x = wm_x; y = wm_y; w = xwAttr2.width; h = xwAttr2.height; ROS_DEBUG_STREAM_COND(log_level > 2, name << " wm changed display " << x << " " << y << ", " << w << " " << h); setSignal("x", x); setSignal("y", y); setSignal("w", w); setSignal("h", h); } else { // see if user changed anything through vimjay interface, // this has lower priority than window manager changes int new_x = getSignal("x"); int new_y = getSignal("y"); int new_w = getSignal("w"); int new_h = getSignal("h"); if ( (x != new_x) || (y != new_y) || (w != new_w) || (h != new_h) ) { ROS_DEBUG_STREAM_COND(log_level > 2, name << " vimjay changed display " << CLVAL << new_x << " " << new_y << ", " << new_w << " " << new_h << CLNRM << ", old " << CLVAL << x << " " << y << ", " << w << " " << h << CLNRM); // int dx = new_x - x; // int dy = new_y - y; x = new_x; y = new_y; w = new_w; h = new_h; XWindowChanges values; unsigned int value_mask = 0x0; values.x = x; values.y = y; // if the window is crammed against the right border, increasing width // results in shrinking the window which is strange values.width = w; values.height = h; ROS_DEBUG_STREAM_COND(log_level > 6, values.x << " " << values.y); // TBD test if different { value_mask = CWWidth | CWHeight; value_mask |= CWX | CWY; XConfigureWindow(display, win, value_mask, &values); } // TBD if (win != toplevel_parent) { // TBD hack, the difference between the toplevel window and the // actual one are overcome by giving all of the offset to win zeroing // out the toplevel, when the wm changes the position it all goes to the toplevel. values.x = 0; values.y = 0; value_mask = CWX | CWY; XConfigureWindow(display, toplevel_parent, value_mask, &values); } setSignal("x", x); setSignal("y", y); setSignal("w", w); setSignal("h", h); } // see if vimjay ui change xywh } // xywh changes cv::Mat in = getImage("in"); // if (in.empty()) return true; if (!(in.empty()) && ximage) { // These aren't used, just provided for convenience // and are set here to make sure the user didn't try to override them // (TBD set to be unwriteable when that functionality is supported setSignal("im_w", Config::inst()->im_width); setSignal("im_h", Config::inst()->im_height); ROS_DEBUG_STREAM_COND(log_level > 5, "attr time" << t1.elapsed()); // TBD is this necessary or does X do it for me if the window is resized? const cv::Size sz = cv::Size(w, h); cv::Mat scaled; if (sz == in.size()) scaled = in.clone(); else cv::resize(in, scaled, sz, 0, 0, getModeType()); ROS_DEBUG_STREAM_COND(log_level > 6, "resize time" << t1.elapsed()); XDestroyImage(ximage); ximage = XGetImage(display, DefaultRootWindow(display), 0, 0, w, h, AllPlanes, ZPixmap); ROS_DEBUG_STREAM_COND(log_level > 5, "get im time" << t1.elapsed()); // this is slow bm::matToXImage(scaled, ximage, win, *display, *screen); ROS_DEBUG_STREAM_COND(log_level > 5, "matToXImage time" << t1.elapsed()); XPutImage(display, win, gc, ximage, 0, 0, 0, 0, ximage->width, ximage->height); ROS_DEBUG_STREAM_COND(log_level > 4, "put image time" << t1.elapsed()); } // ximage and input image is valid #endif return ImageNode::draw(ui_offset); }