void userCallback(InteractiveRobot& robot) { // move the world geometry in the collision world Eigen::Affine3d world_cube_pose; double world_cube_size; robot.getWorldGeometry(world_cube_pose, world_cube_size); g_planning_scene->getWorldNonConst()->moveShapeInObject("world_cube", g_world_cube_shape, world_cube_pose); // prepare to check collisions collision_detection::CollisionRequest c_req; collision_detection::CollisionResult c_res; c_req.group_name = "right_gripper"; //c_req.group_name = "right_arm"; c_req.contacts = true; c_req.max_contacts = 100; c_req.max_contacts_per_pair = 5; c_req.verbose = true; // check for collisions between robot and itself or the world g_planning_scene->checkCollision(c_req, c_res, *robot.robotState()); // display results of the collision check if (c_res.collision) { ROS_INFO("COLLIDING contact_point_count=%d",(int)c_res.contact_count); // get the contact points and display them as markers if (c_res.contact_count > 0) { std_msgs::ColorRGBA color; color.r = 1.0; color.g = 0.0; color.b = 1.0; color.a = 0.5; visualization_msgs::MarkerArray markers; collision_detection::getCollisionMarkersFromContacts(markers, "base_footprint", c_res.contacts, color, ros::Duration(), // remain until deleted 0.01); // radius publishMarkers(markers); } } else { ROS_INFO("Not colliding"); // delete the old collision point markers visualization_msgs::MarkerArray empty_marker_array; publishMarkers(empty_marker_array); } }
bool ObjectMatching::matchCallback(tangible_msgs::GetMatchingObjects::Request& req, tangible_msgs::GetMatchingObjects::Response& res) { ROS_INFO("Got service call!"); if (req.target.type == tangible_msgs::Target::POINT_LOCATION){ ROS_INFO("Matching object for target type POINT_LOCATION"); for (int i=0; i<req.objects.size(); i++){ if (matchesPointLocation(req.objects[i], req.target)){ res.objects.push_back(req.objects[i]); } } } else if (req.target.type == tangible_msgs::Target::REGION) { ROS_INFO("Matching object for target type REGION"); for (int i=0; i<req.objects.size(); i++){ if (matchesRegion(req.objects[i], req.target)){ res.objects.push_back(req.objects[i]); } } } else if (req.target.type == tangible_msgs::Target::OBJECT_SELECTOR) { ROS_INFO("Matching object for target type OBJECT_SELECTOR"); for (int i=0; i<req.objects.size(); i++){ if (matchesObjSelector(req.objects[i], req.target)){ res.objects.push_back(req.objects[i]); } } } publishMarkers(); return true; }
bool HapticsPSM::check_collison(){ coll_res.clear(); psm_planning_scene->checkCollisionUnpadded(coll_req,coll_res,*group->getCurrentState(), psm_planning_scene->getAllowedCollisionMatrix()); if (coll_res.collision) { //ROS_INFO("COLLIDING contact_point_count=%d",(int)coll_res.contact_count); if (coll_res.contact_count > 0) { std_msgs::ColorRGBA color; color.r = 1.0; color.g = 0.0; color.b = 0.2; color.a = 0.8; visualization_msgs::MarkerArray markers; collision_detection::getCollisionMarkersFromContacts(markers, group->getPlanningFrame().c_str(), coll_res.contacts, color, ros::Duration(), // remain until deleted 0.002); publishMarkers(markers); } return true; } else { //ROS_INFO("Not colliding"); // delete the old collision point markers visualization_msgs::MarkerArray empty_marker_array; publishMarkers(empty_marker_array); return false; } }
void WolfNode::solve() { // Solving --------------------------------------------------------------------------- //ROS_INFO("================ SOLVING =================="); ceres::Solver::Summary summary = ceres_manager_.solve(); //std::cout << "------------------------- SOLVED -------------------------" << std::endl; //std::cout << summary.BriefReport() << std::endl; //summary.FullReport() // Broadcast transforms --------------------------------------------------------------------------- broadcastTf(); // MARKERS VEHICLE & CONSTRAINTS --------------------------------------------------------------------------- publishMarkers(); }
bool AudioProcessor::processFrame() { // TODO: check whether this reallocates memory frequency_histogram_.setZero(static_cast<Eigen::DenseIndex>(number_of_frequency_bins_)); for (int i = 0; i < num_frequencies_bins_; i++) { for (int j = 0; j < num_frequencies_per_bin_; ++j) { frequency_histogram_(i) = frequency_histogram_(i) + spectrum_[i * num_frequencies_per_bin_ + j]; } } if(publish_visualization_markers_ && (frame_count_ % visualization_publishing_ratio_) == 0) { publishMarkers(); } return true; }
void LoadEstimator::publish_inertial_data() { while(going_) { mutex_thread_.lock(); double norm_of_P = P_.norm(); std::cout << "norm of P_ \t" << norm_of_P << std::endl << std::endl; computeInertialParameters(); mutex_thread_.unlock(); //we don;t need the lock here inertial_estimation_pub_.publish(inertial_parameters_msg_); //Publish markers publishMarkers(); ros::Duration(pub_time_).sleep(); } }