void ObjRecInterface::recognize_objects() { while(ros::ok() && !time_to_stop_) { // Don't hog the cpu ros::Duration(0.03).sleep(); // Scope for syncrhonization pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>()); { // Lock the buffer mutex boost::mutex::scoped_lock buffer_lock(buffer_mutex_); // Continue if the cloud is empty static ros::Rate warn_rate(1.0); if(clouds_.empty()) { ROS_WARN("Pointcloud buffer is empty!"); warn_rate.sleep(); continue; } ros::Time time_back, time_front; time_back.fromNSec(clouds_.back()->header.stamp * 1000); time_front.fromNSec(clouds_.back()->header.stamp * 1000); ROS_INFO_STREAM("Computing objects from " <<scene_points_->GetNumberOfPoints()<<" points " <<"between "<<(ros::Time::now() - time_back) <<" to "<<(ros::Time::now() - time_front)<<" seconds after they were acquired."); // Copy references to the stored clouds cloud_full->header = clouds_.front()->header; while(!clouds_.empty()) { *cloud_full += *(clouds_.front()); clouds_.pop(); } } objrec_msgs::RecognizedObjects objects_msg = do_recognition(cloud_full); // Publish the visualization markers this->publish_markers(objects_msg); // Publish the recognized objects objects_pub_.publish(objects_msg); // Publish the points used in the scan, for debugging /** pcl::PointCloud<pcl::PointXYZRGB>::Ptr foreground_points_pcl(new pcl::PointCloud<pcl::PointXYZRGB>()); foreground_points_pcl->header = cloud->header; for(unsigned int i=0; i<foreground_points->GetNumberOfPoints(); i++) { double point[3]; foreground_points->GetPoint(i,point); foreground_points_pcl->push_back(pcl::PointXYZ(point[0]/1000.0,point[1]/1000.0,point[2]/1000.0)); } foreground_points_pub_.publish(foreground_points_pcl); **/ } }
//================================================================ // Close the gripper at the specified rate to the distance // specified //================================================================ void closeToPosition(ros::Rate rate, double move_gripper_distance, double gripper_position) { // Get location of gripper currently double current_gripper_position = simple_gripper->getGripperLastPosition(); int pressure_max = 0; // Continue until position is achieved, ros cancel, or if // the pressure approaches something dangerous while (current_gripper_position > gripper_position && pressure_max < 500 && current_gripper_position > 0.0 && ros::ok()) { // Move the gripper by the specified amount simple_gripper->closeByAmount(move_gripper_distance); // Find and update if gripper moved current_gripper_position = simple_gripper->getGripperLastPosition(); // Get pressure pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); // Wait set time and check again ros::spinOnce(); rate.sleep(); } }
position_tracker::Position take_step(int action, ros::Rate loop_rate, ros::ServiceClient client) { printf("p take action\n"); cout << "taking action " <<action << endl; /*Execute the Action */ if(action==LEFT){ left(client); } else if(action==FORWARD){ forward(client); } else{ right(client); } cout << "took an action" << endl; ros::spinOnce(); loop_rate.sleep(); //check to make sure this observation is ok position_tracker::Position observation= getObservation(); return observation; /* cout << "going to convert"<< endl; convertObservation(observation); cout << "converted"<<endl; this_reward_observation.reward= calculateReward(observation); this_reward_observation.terminal=checkTerminal(observation); cout << "we took a step" << endl; return &this_reward_observation; */ }
//================================================================ // Close gripper to specified pressure //================================================================ void closeToPressure(ros::Rate rate, int desired_pressure, double move_gripper_distance) { double current_gripper_position = simple_gripper->getGripperLastPosition(); int pressure_max = 0; int pressure_min = 0; while (pressure_min < desired_pressure && pressure_max < 500 && ros::ok() && current_gripper_position > 0.0) { // Move the gripper by the specified amount simple_gripper->closeByAmount(move_gripper_distance); // Find and update if gripper moved current_gripper_position = simple_gripper->getGripperLastPosition(); // Get pressure pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); // Wait set time and check again ros::spinOnce(); rate.sleep(); } }
void turn(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){ int count = 0; while(ros::ok()){ count++; out.publish(msg); rate.sleep(); if (count == 40)break; //after 45 updates break } }
void moveForward(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){ int count =0; //number of updates while(ros::ok()){ out.publish(msg); rate.sleep(); count++; if (count == 10) break; //after a second break } }
void Node::mapPublishThread(ros::Rate rate) { while(ros::ok()) { { #ifdef USE_HECTOR_TIMING hector_diagnostics::TimingSection section("map publish"); #endif publishMap(); } rate.sleep(); } }
// TODO needs some love void run() { while(ros::ok()) { current_time = ros::Time::now(); double dt = (current_time - last_time).toSec(); // TODO these predicst and measure variables are not something that I understand if (predict) { prediction(dt); last_time = current_time; predict = false; if (measure) { update_estimated_measurement(); weighting(); measure = false; } Normalization(); resample(); } if (visualization_enabled) { if (publish_particles) { particles_pub.publish(loc_viz.get_particle_marker(particle_state)); } if (publish_robot) { robot_pub.publish( loc_viz.get_robot_marker(std::vector<ras::sensor_info>(), x, y, theta, visualization_use_distance)); } if (publish_thickened_walls) { wall_cell_pub.publish(loc_viz.get_thick_wall_grid_cells()); } if (publish_walls) { grid_cell_pub.publish(loc_viz.get_wall_grid_cells()); } if (publish_path) { point_path_pub.publish(loc_viz.add_to_path(x, y)); } } //Publish Geometry msg of Predicted postion geometry_msgs::Pose2D msg; msg.x = x; msg.y = y; msg.theta = theta; position_pub.publish(msg); //Use if add a subscription(Add as good measure) ros::spinOnce(); //Delays untill it is time to send another message rate->sleep(); last_time = current_time; } }
//================================================================ // Closes the gripper until a pressure is achieved, // then opens again at the same rate the gripper closed at //================================================================ void squeeze(ros::Rate rate, double move_gripper_distance) { int previous_pressure_max = 0; int pressure_max = 0; int no_motion_counter = 0; // Close while (pressure_max < SqueezePressureContact && ros::ok() && no_motion_counter < 250 && simple_gripper->getGripperLastPosition() > 0.0) { previous_pressure_max = pressure_max; pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); // Checks if pressure has been "stuck" if (abs(previous_pressure_max-pressure_max) < 1) no_motion_counter++; simple_gripper->closeByAmount(move_gripper_distance); //ROS_INFO("Pressure Max is: [%d]", pressure_max); ros::spinOnce(); rate.sleep(); } // Store the gripper position when max pressure is achieved gripper_max_squeeze_position = simple_gripper->getGripperLastPosition(); // Open - 10 and not 0 because the values will drift while (pressure_max > 10 && ros::ok() && simple_gripper->getGripperLastPosition() < 0.08) { pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); simple_gripper->openByAmount(move_gripper_distance); //ROS_INFO("Pressure Max is: [%d]", pressure_max); ros::spinOnce(); rate.sleep(); } }
position_tracker::Position start_environment(ros::Rate loop_rate) { /* see where the robot is. And then send that position*/ ros::spinOnce(); loop_rate.sleep(); position_tracker::Position observation= getObservation(); cout << observation.x << " " << observation.y << " " << observation.theta << endl; return observation; }
//================================================================ // Open gripper by the rate and position specified. // This is necessary to keep opening the gripper until // the bioTacs do not report any pressure //================================================================ void openUntilNoContact(ros::Rate rate, double gripper_position) { int pressure_max = LightPressureContact + 50; while (pressure_max > LightPressureContact && ros::ok()) { pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); simple_gripper->open2Position(gripper_position); //ROS_INFO("Pressure Max is: [%d]", pressure_max); ros::spinOnce(); rate.sleep(); } }
int main(int argc, char** argv) { ros::init(argc, argv,"button_led"); ROS_INFO("Started Odroid-C1 Button Blink Node"); wiringPiSetup (); pinMode(LED, OUTPUT); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("led_blink",10,blink_callback); ros::Publisher chatter_pub = n.advertise<std_msgs::Bool>("led_blink", 10); std_msgs::Bool on = true; std_msgs::Bool off = false; while (ros::ok()) { if (digitalRead(butPin)) // Button is released if this returns 1 { chatter_pub.publish(on); } else { chatter_pub.publish(off); } ros::spinOnce(); loop_rate.sleep(); } }
void run(){ RNG rng(12345); while(ros::ok()) { current_time = ros::Time::now(); //computer odemetry using integration double dt = (current_time - last_time).toSec(); //Prediction model Prediction(dt); //Measurement model EstimateMeasurement_Update(); RealMeasurement_Update(); Innovation(); ReSampling(); Normalization(); // Show all the particles on the map image = imread(PATH,CV_LOAD_IMAGE_COLOR); Scalar color = Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)); for(int i=0;i<Particles;++i){ particle_position = Point(particle_state[1][i]*100+5,particle_state[0][i]*100+5); circle(image,particle_position,4,color,-1,8,0); } // Show the final predicted position on the map circle(postion_image,position,4,color,-1,8,0); //Show odometry localization on the map namedWindow( OPENCV_WINDOW, CV_WINDOW_NORMAL );// Create a window for display. imshow( OPENCV_WINDOW, image ); // Show our map inside it. namedWindow( OPENCV_WINDOW1, CV_WINDOW_NORMAL );// Create a window for display. imshow( OPENCV_WINDOW1, postion_image); //Publish Geometry msg of Predicted postion msg.x = x; msg.y = y; msg.theta = theta; position_pub.publish(msg); waitKey(3); // Wait for a keystroke in the window last_time = current_time; //Use if add a subscription(Add as good measure) ros::spinOnce(); //Delays untill it is time to send another message rate->sleep(); } }
//Moves forward for 3 seconds, moves backwards 3 seconds //Press Enter for emergency land void controller::testMove(ros::Rate loop_rate, ros::NodeHandle node) { if (inFlight == 0) { controller::sendTakeoff(node); inFlight = 1; } start_time = (double)ros::Time::now().toSec(); cout << "Started Time: " << start_time << "\n" << endl; linebufferedController(false); echoController(false); while ((double)ros::Time::now().toSec() < start_time + test_flight_time) { if ((double)ros::Time::now().toSec() < start_time + test_flight_time / 2) { controller::setMovement(5, 0, 0); controller::sendMovement(node); } else { controller::setMovement(-5,0, 0); controller::sendMovement(node); } ros::spinOnce(); loop_rate.sleep(); if (iskeypressed(500) && cin.get() == '\n') break; } controller::resetTwist(); controller::sendMovement(node); controller::sendLand(node); echoController(); linebufferedController(); }
void computeBackgroundCloud (int frames, float voxel_size, int sr_conf_threshold, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud) { std::cout << "Background acquisition..." << std::flush; // Create background cloud: PointCloudT::Ptr cloud_to_process(new PointCloudT); background_cloud->header = cloud->header; for (unsigned int i = 0; i < frames; i++) { *cloud_to_process = *cloud; // Remove low confidence points: removeLowConfidencePoints(confidence_image, sr_conf_threshold, cloud_to_process); // Voxel grid filtering: PointCloudT::Ptr cloud_filtered(new PointCloudT); pcl::VoxelGrid<PointT> voxel_grid_filter_object; voxel_grid_filter_object.setInputCloud(cloud_to_process); voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size); voxel_grid_filter_object.filter (*cloud_filtered); *background_cloud += *cloud_filtered; ros::spinOnce(); rate.sleep(); } // Voxel grid filtering: PointCloudT::Ptr cloud_filtered(new PointCloudT); pcl::VoxelGrid<PointT> voxel_grid_filter_object; voxel_grid_filter_object.setInputCloud(background_cloud); voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size); voxel_grid_filter_object.filter (*cloud_filtered); background_cloud = cloud_filtered; // Background saving: pcl::io::savePCDFileASCII ("/tmp/background_" + frame_id.substr(1, frame_id.length()-1) + ".pcd", *background_cloud); std::cout << "done." << std::endl << std::endl; }
hmc5883l::hmc5883l(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, hmc5883l_callBackFunc dataReadyCallBack = 0) { ROS_INFO("HMC5883L : Initializing"); char str[80]; i2c = i2c_ptr; sem_init(&lock, 0, 1); ctl_reg_a = (0x03 << HMC5883L_MA0) | (0x06 << HMC5883L_DO0) | (0x00 << HMC5883L_MS0); i2c->write_byte(addr, HMC5883L_CONF_REG_A, ctl_reg_a); mode_reg = (0x00 << HMC5883L_MD0); i2c->write_byte(addr, HMC5883L_MODE_REG, mode_reg); setScale(hmc5883l_scale_130); nh = nh_ptr; timer = nh->createTimer(rate, &hmc5883l::timerCallback, this); if (dataReadyCallBack) dataCallback = dataReadyCallBack; sprintf(str, "Sampling @ %2.2f Hz", (float) 1.0f / rate.expectedCycleTime().toSec()); ROS_INFO("HMC5883L : \t%s", str); ROS_INFO("HMC5883L : Initialization done"); }
micromag::micromag(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, micromag_callBackFunc dataReadyCallBack = 0, int windowSize = 32) { ROS_INFO("micromag : Initializing"); char str[80]; i2c = i2c_ptr; sem_init(&lock, 0, 1); double freq = 1 / rate.expectedCycleTime().toSec(); setWindow(windowSize); nh = nh_ptr; timer = nh->createTimer(rate, µmag::timerCallback, this); if (dataReadyCallBack) dataCallback = dataReadyCallBack; sprintf(str, "Sampling @ %2.2f Hz", freq); ROS_INFO("micromag : \t%s", str); ROS_INFO("micromag : Initialization done"); // Probe device... Return 0 on failure.. }
//================================================================ // Higher level motion to close gripper until contact is found // Pass in the rate at which contact is closed at and the // distance the gripper moves rate is in Hz, // move_gripper_distance in meters // Velocity gripper moves is found by how much moved by what rate //================================================================ void findContact(ros::Rate rate, double move_gripper_distance) { int pressure_min = 0; int pressure_max = 0; bool contact_found = false; int no_motion_counter = 0; int previous_pressure_max = 0; // Close until minimum pressure is found - however stop if // any finger has too much pressure while (pressure_min < LightPressureContact && ros::ok() && pressure_max < 600 && no_motion_counter < 250) { // Check pressure min and max simple_gripper->closeByAmount(move_gripper_distance); pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); // Checks if pressure has been "stuck" if (abs(previous_pressure_max-pressure_max) < 1) no_motion_counter++; // Set distance for object width if (!contact_found && pressure_min > 10){ gripper_initial_contact_position = simple_gripper->getGripperLastPosition(); contact_found = true; } previous_pressure_max = pressure_max; //ROS_INFO("Pressure Min is: [%d]", pressure_min); // ROS_INFO("Pressure Max is: [%d]", pressure_max); ros::spinOnce(); rate.sleep(); } }
//================================================================ // Function to move the arm a little in a direction it needs to // move //================================================================ void centerArmIncremental(ros::Rate rate, double move_gripper_distance) { // Find position of arm arm_controller->getArmTransform(); double x = arm_controller->getTransform('x'); double y = arm_controller->getTransform('y'); double z = arm_controller->getTransform('z'); int pressure_min = 0; int pressure_max = 0; bool fingerSet = false; int num_run = 0; bool not_centered = true; int pressure_left = 0; int pressure_right = 0; int move_counter = 0; int lastFingerContact = 5; double distance_move_arm = 0.005; // Close until minimum pressure is found - however stop if // any finger has too much pressure while (num_run < 8 && not_centered && pressure_max < 600 && ros::ok()) { simple_gripper->closeByAmount(move_gripper_distance); pressure_left = biotac_obs->pressure_normalized_[Left]; pressure_right = biotac_obs->pressure_normalized_[Right]; // Check pressure min and max pressure_min = min(pressure_left, pressure_right); pressure_max = max(pressure_left, pressure_right); // First touches object if (pressure_max > 5 && !fingerSet) { fingerSet = true; if (pressure_min > 5) { not_centered = false; fingerSet = false; } firstContact.position = simple_gripper->getGripperLastPosition(); if (pressure_left > pressure_right) { firstContact.finger = Left; } else { firstContact.finger = Right; } // Checks if finger was not set yet if (lastFingerContact == 5) { lastFingerContact = firstContact.finger; } // Stop if finger contact changed if (lastFingerContact != firstContact.finger) { not_centered = false; } } if (fingerSet) { if (!not_centered) { distance_move_arm = 0.0025; } // Find position of arm arm_controller->getArmTransform(); x = arm_controller->getTransform('x'); y = arm_controller->getTransform('y'); z = arm_controller->getTransform('z'); // Open grippers simple_gripper->open2Position(GripperMaxOpenPosition); // Move left case if (firstContact.finger == Left) { arm_controller->move_arm_to(x,y+distance_move_arm,z,1); } else { arm_controller->move_arm_to(x,y-distance_move_arm,z,1); } fingerSet = false; num_run++; move_counter = 0; } //ROS_INFO("Pressure Min is: [%d]", pressure_min); //ROS_INFO("Pressure Max is: [%d]", pressure_max); //ROS_INFO("Left finger is: [%d]", pressure_left); //ROS_INFO("Right finger is: [%d]", pressure_right); move_counter++; ros::spinOnce(); rate.sleep(); } }
void runLocalController() { while(ros::ok()) { rate->sleep(); //If we are receiving data switch(currentState) { case STOPPED: //ROS_ERROR("STOPPED!"); //Next state if(distanceToPerson > planner_activation_distance && hold == false) { nextState = PLANNER; //delete rate; //rate = new ros::Rate(50); geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); } else if(distanceToPerson < planner_activation_distance && distanceToPerson > minimum_distance && hold == false) { nextState = LOCALCONTROLLER; //delete rate; //rate = new ros::Rate(50); geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); } /*else { nextState = STOPPED; geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); }*/ break; case PLANNER: //ROS_ERROR("PLANNER"); if(distanceToPerson >= minimum_planner_distance && hold == false) { nextState = PLANNER; //Get transforms cv::Point3d personPoint; personPoint.x = personInRobotBaseFrame.point.x; personPoint.y = personInRobotBaseFrame.point.y; personPoint.z = personInRobotBaseFrame.point.z; segwayController::proportionalController(personPoint, cmdPub, kv, kalfa); } else if( distanceToPerson < minimum_planner_distance && distanceToPerson >= minimum_distance && hold == false) { nextState = LOCALCONTROLLER; //delete rate; //rate = new ros::Rate(50); geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); } else if(distanceToPerson < minimum_distance || hold == true) { nextState = STOPPED; geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); //delete rate; //rate = new ros::Rate(50); } break; case LOCALCONTROLLER: //ROS_ERROR("LOCALCONTROLLER"); if(distanceToPerson >= minimum_distance && distanceToPerson < planner_activation_distance && hold == false) { nextState = LOCALCONTROLLER; cv::Point3d personPoint; personPoint.x = personInRobotBaseFrame.point.x; personPoint.y = personInRobotBaseFrame.point.y; personPoint.z = personInRobotBaseFrame.point.z; //Same controller, but no v segwayController::proportionalController(personPoint, cmdPub, 0, kalfa); } else if(distanceToPerson < minimum_distance || hold == true) { nextState = STOPPED; geometry_msgs::Twist goal; goal.angular.x = 0; goal.angular.y = 0; goal.angular.z = 0; goal.linear.x = 0; goal.linear.y = 0; goal.linear.z = 0; cmdPub.publish(goal); } else if(distanceToPerson >= planner_activation_distance) { nextState = PLANNER; //delete rate; //rate = new ros::Rate(50); } break; } ros::spinOnce(); currentState = nextState; } }
void TargetPointSender::sleep(){ rate_.sleep(); ros::spinOnce(); }
void spinOnce(ros::Rate& loopRate) { loopRate.sleep(); //Maintain the loop rate ros::spinOnce(); //Check for new messages }
void MTMHaptics::set_effort_mode(){ robot_state_cmd.data = "DVRK_EFFORT_CARTESIAN"; robot_state_pub.publish(robot_state_cmd); ROS_INFO("Setting Robot state as: %s",robot_state_cmd.data.c_str()); rate_->sleep(); }
void ObjRecInterface::recognize_objects_thread() { ros::Rate max_rate(100.0); // Working structures pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>()); boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZRGB> > voxel_grid(new pcl::VoxelGrid<pcl::PointXYZRGB>()); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::PointIndices::Ptr outliers (new pcl::PointIndices); // Create point clouds for foreground and background points vtkSmartPointer<vtkPoints> foreground_points; foreground_points.TakeReference(vtkPoints::New(VTK_DOUBLE)); std::list<boost::shared_ptr<PointSetShape> > detected_models; while(ros::ok() && !time_to_stop_) { // Don't hog the cpu max_rate.sleep(); cloud_full->clear(); cloud->clear(); ROS_DEBUG_STREAM("ObjRec: Aggregating point clouds... "); { // Scope for syncrhonization // Continue if the cloud is empty static ros::Rate warn_rate(1.0); if(clouds_.empty()) { ROS_WARN("Pointcloud buffer is empty!"); warn_rate.sleep(); continue; } // Lock the buffer mutex boost::mutex::scoped_lock buffer_lock(buffer_mutex_); ROS_DEBUG_STREAM("ObjRec: Computing objects from " <<clouds_.size()<<" point clounds " <<"between "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.back()->header).stamp) <<" to "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.front()->header).stamp)<<" seconds after they were acquired."); // Copy references to the stored clouds cloud_full->header = clouds_.front()->header; for(std::list<boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > >::const_iterator it = clouds_.begin(); it != clouds_.end(); ++it) { *cloud_full += *(*it); } } // Recongize objects bool objects_recognized = this->recognize_objects( cloud_full, cloud, voxel_grid, coefficients, inliers, outliers, foreground_points, detected_models, true, true); // No objects recognized if(!objects_recognized) { continue; } // Construct recognized objects message objrec_msgs::RecognizedObjects objects_msg; objects_msg.header.stamp = pcl_conversions::fromPCL(cloud->header).stamp; objects_msg.header.frame_id = "/world";//cloud->header; for(std::list<boost::shared_ptr<PointSetShape> >::iterator it = detected_models.begin(); it != detected_models.end(); ++it) { boost::shared_ptr<PointSetShape> detected_model = *it; // Construct and populate a message objrec_msgs::PointSetShape pss_msg; pss_msg.label = detected_model->getUserData()->getLabel(); pss_msg.confidence = detected_model->getConfidence(); array_to_pose(detected_model->getRigidTransform(), pss_msg.pose); // Transform into the world frame TODO: make this frame a parameter geometry_msgs::PoseStamped pose_stamped_in, pose_stamped_out; pose_stamped_in.header = pcl_conversions::fromPCL(cloud->header); pose_stamped_in.pose = pss_msg.pose; try { listener_.transformPose("/world",pose_stamped_in,pose_stamped_out); pss_msg.pose = pose_stamped_out.pose; } catch (tf::TransformException ex){ ROS_WARN("Not transforming recognized objects into world frame: %s",ex.what()); } objects_msg.objects.push_back(pss_msg); } // Publish the visualization markers this->publish_markers(objects_msg); // Publish the recognized objects objects_pub_.publish(objects_msg); // Publish the points used in the scan, for debugging foreground_points_pub_.publish(cloud); } }
void receive(ros::Rate & loop_rate) { for (unsigned i = 0u; i < 10u; i++){ ros::spinOnce(); loop_rate.sleep(); } }
//================================================================ // Higher level motion to close gripper until contact is found // Pass in the rate at which contact is closed at and the // distance the gripper moves rate is in Hz, // move_gripper_distance in meters // Velocity gripper moves is found by how much moved by what rate //================================================================ void findContactOld(ros::Rate rate, double move_gripper_distance) { int pressure_min = 0; int pressure_max = 0; bool contact_found = false; bool fingerSet = true; int no_motion_counter = 0; int previous_pressure_max = 0; // Close until minimum pressure is found - however stop if // any finger has too much pressure while (pressure_min < LightPressureContact && ros::ok() && pressure_max < 600 && no_motion_counter < 250) { previous_pressure_max = pressure_max; // Checks if pressure has been "stuck" if (abs(previous_pressure_max-pressure_max) < 1) no_motion_counter++; // First touches object if (pressure_max > 5 && fingerSet) { firstContact.position = simple_gripper->getGripperLastPosition(); if (biotac_obs->pressure_normalized_[Left] > biotac_obs->pressure_normalized_[Right]) { firstContact.finger = Left; } else { firstContact.finger = Right; } fingerSet = false; } // Second finger touches object if (pressure_min > 10) { secondContact.position = simple_gripper->getGripperLastPosition(); if (firstContact.position == Left) { secondContact.finger = Right; } else { secondContact.finger = Left; } } // Set distance for object width if (!contact_found && pressure_min > 10){ gripper_initial_contact_position = simple_gripper->getGripperLastPosition(); contact_found = true; } // Store last pressure felt by each finger tap_pressure_left = biotac_obs->pressure_normalized_[Left]; tap_pressure_right = biotac_obs->pressure_normalized_[Right]; // Check pressure min and max pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]); simple_gripper->closeByAmount(move_gripper_distance); //ROS_INFO("Pressure Min is: [%d]", pressure_min); // ROS_INFO("Pressure Max is: [%d]", pressure_max); ros::spinOnce(); rate.sleep(); } }
void runPlanner() { while(ros::ok()) { rate->sleep(); //If we are receiving data switch(currentState) { case STOPPED: //ROS_ERROR("STOPPED!"); //Next state if(distanceToPerson > planner_activation_distance && hold == false) { nextState = PLANNER; delete rate; rate = new ros::Rate(10); } break; case PLANNER: ROS_ERROR("PLANNER"); if(distanceToPerson >= minimum_planner_distance && hold == false) { nextState = PLANNER; //Get transforms tf::StampedTransform transform; try { listener.waitForTransform(world_frame, robot_frame, ros::Time(0), ros::Duration(10.0) ); listener.lookupTransform(world_frame, robot_frame,ros::Time(0), transform); } catch(tf::TransformException ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } Eigen::Affine3d eigen_transform; tf::transformTFToEigen(transform, eigen_transform); // convert matrix from Eigen to openCV cv::Mat baseLinkToMap; cv::eigen2cv(eigen_transform.matrix(), baseLinkToMap); cv::Point3d personPoint; personPoint.x = personInRobotBaseFrame.point.x; personPoint.y = personInRobotBaseFrame.point.y; personPoint.z = personInRobotBaseFrame.point.z; segwayController::moveBase(personPoint, baseLinkToMap, ac, distance_to_target); } else if(distanceToPerson < minimum_planner_distance || hold == true) { nextState = STOPPED; ac->cancelAllGoals(); delete rate; rate = new ros::Rate(10); } break; default: nextState = STOPPED; ac->cancelAllGoals(); } ros::spinOnce(); currentState = nextState; } }