void Network::sendScantoMap(gtsam::Pose3 pose, sensor_msgs::PointCloud2 scan) { mapPub.publish(scan); sensor_msgs::PointCloud::Ptr scan_body(new sensor_msgs::PointCloud); sensor_msgs::convertPointCloud2ToPointCloud(scan, *scan_body); //mapPub.publish(scan_body); gtsam::Vector3 rpy = pose.rotation().rpy(); tf::Quaternion q; q.setRPY(rpy(0), rpy(1), rpy(2)); geometry_msgs::Quaternion qmsg; tf::quaternionTFToMsg(q,qmsg); geometry_msgs::Pose pose_msg; pose_msg.orientation = qmsg; pose_msg.position.x = pose.x(); pose_msg.position.y = pose.y(); pose_msg.position.z = pose.z(); geometry_msgs::PoseStamped stamped_pose; stamped_pose.pose = pose_msg; stamped_pose.header.frame_id = "world"; stamped_pose.header.stamp = ros::Time::now(); posePub.publish(stamped_pose); dvm_.addRegisteredPointCloud(pose_msg,scan_body,range_max_); }
VectorXd MotionModel::tr2rpy(MatrixXd m) { VectorXd rpy(3); if (abs(m(0,0)) < eps && abs(m(1,0)) < eps) rpy << 0, atan2(-m(2,0), m(0,0)), atan2(-m(1,2), m(1,1)); else { double sp, cp; sp = sin(atan2(m(1,0), m(0,0))); cp = cos(atan2(m(1,0), m(0,0))); rpy << atan2(m(1,0), m(0,0)), atan2(-m(2,0), cp * m(0,0) + sp * m(1,0)), atan2(sp * m(0,2) - cp * m(1,2), cp * m(1,1) - sp * m(0,1)); } return rpy; }
int main(int argc,char** argv){ ww::World_wrapper world_wrapper; geo::fCVec3 origin_ = {{0,0,-0.02/2 - 0.03/2}}; geo::fCVec3 dim_ = {{0.8,0.4,0.05}}; geo::fCVec3 orientation_ = {{M_PI/2,0,M_PI/2}}; wobj::WBox wsocket_wall("socket_wall",dim_,origin_,orientation_); /// add a socket tf::Vector3 origin(0,0,0); tf::Vector3 rpy(M_PI/2,0,M_PI/2); obj::Socket_one socket_one("socket_one",origin,rpy,1); world_wrapper.wrapped_objects.push_back_box(wsocket_wall); world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket); world_wrapper.wrapped_objects.push_back_box(socket_one.wbox); plugfilter::PF_parameters pf_parameters(world_wrapper.wrapped_objects); pf_parameters.number_particles = 4000; Sensor_parameters sensor_parameters(world_wrapper.wrapped_objects); sensor_parameters.t_sensor = psm::SIMPLE_CONTACT_DIST; Run_trajectories run_trajectories; // run_trajectories.setup(pf_parameters,sensor_parameters); std::string folder_name = "/home/guillaume/MatlabWorkSpace/peg_in_hole/TextData"; std::string subject_name = "Albert"; std::string plug_type = "A"; std::size_t i = 1; std::string folder_path = folder_name + "/" + subject_name + "/"; std::string file_name = subject_name + "_" + plug_type + "_" + boost::lexical_cast<std::string>(i) + "_.txt"; run_trajectories.load(folder_path,file_name); return 0; }
void Peg_world_wrapper::initialise_socket(const std::string& socket_link_name,const std::string& wall_link_name){ tf::StampedTransform transform; opti_rviz::Listener::get_tf_once(fixed_frame,socket_link_name,transform); opti_rviz::Listener::print(transform); /// add a socket tf::Vector3 origin = transform.getOrigin(); tf::Vector3 rpy(M_PI/2,0,M_PI/2); socket_one = obj::Socket_one(socket_link_name,wall_link_name,origin,rpy,1); world_wrapper.wrapped_objects.push_back_box(&(socket_one.wbox)); world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket); world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[0])); world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[1])); world_wrapper.wrapped_objects.push_back_box(&(socket_one.hole_wboxes[2])); }
static inline std::vector<YAML::Node> get_frame_tip_nodes(const KDL::Segment& seg) { std::vector<YAML::Node> frames; KDL::Frame frame = seg.getFrameToTip() * seg.getJoint().pose(0).Inverse(); // Set xyz if(!KDL::Equal(frame.p, KDL::Vector::Zero())) { YAML::Node xyz_frame; YAML::Node axis_angle_null; axis_angle_null["axis-angle"].push_back(get_vector3(1, 0, 0)); axis_angle_null["axis-angle"].push_back(0); xyz_frame["frame"].push_back(axis_angle_null); xyz_frame["frame"].push_back(get_vector3(frame.p)); frames.push_back(xyz_frame); } // Set rpy if(!KDL::Equal(frame.M, KDL::Rotation::Identity())) { std::vector<double> rpy(3); frame.M.GetRPY(rpy[0],rpy[1],rpy[2]); for (std::vector<double>::size_type i = rpy.size() - 1; i != (std::vector<double>::size_type) -1; i--) { if(rpy[i] != 0) { YAML::Node rpy_frame; YAML::Node axis_angle; KDL::Vector axis; axis[i] = 1; axis_angle["axis-angle"].push_back(get_vector3(axis)); axis_angle["axis-angle"].push_back(rpy[i]); rpy_frame["frame"].push_back(axis_angle); rpy_frame["frame"].push_back(get_vector3(0, 0, 0)); frames.push_back(rpy_frame); } } } return frames; }
int main(int argc, char* argv[]) { ros::init(argc,argv,"navsts2odom"); ros::NodeHandle nh, ph("~"); //Get rotation between the two std::vector<double> rpy(3,0); ph.param("rpy",rpy,rpy); //Setup the LTP to Odom frame Eigen::Quaternion<double> q; labust::tools::quaternionFromEulerZYX(rpy[0], rpy[1], rpy[2],q); Eigen::Matrix3d rot = q.toRotationMatrix().transpose(); ros::Publisher odom = nh.advertise<nav_msgs::Odometry>("uwsim_hook", 1); ros::Subscriber navsts = nh.subscribe<auv_msgs::NavSts>("navsts",1, boost::bind(&onNavSts, boost::ref(odom), boost::ref(rot), _1)); ros::spin(); return 0; }
void transformCam2Robot (const pcl::PointCloud<pcl::PointXYZRGB> &cloud_in, pcl::PointCloud<pcl::PointXYZRGB> &cloud_out) { if (&cloud_in != &cloud_out) { cloud_out = cloud_in; } // Sensor settings Eigen::Vector3d rpy(0, 0.18, 3.14); Eigen::Vector4d xyz(mobile_base_pose.position.x, mobile_base_pose.position.y, mobile_base_pose.position.z, 1); Eigen::Matrix3d R; tf::Quaternion qt = tf::createQuaternionFromRPY(-rpy[1],-rpy[0],-rpy[2]); tf::Matrix3x3 R1(qt); tf::matrixTFToEigen(R1,R); Eigen::Matrix4d tf_matrix; tf_matrix.setZero(); tf_matrix.block (0, 0, 3, 3) = R; tf_matrix (3, 3) = 1; Eigen::Matrix4d tf_matrix_swap; tf_matrix_swap << 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1; double roll, pitch, yaw; tf::Quaternion q( mobile_base_pose.orientation.x, mobile_base_pose.orientation.y, mobile_base_pose.orientation.z, mobile_base_pose.orientation.w ); tf::Matrix3x3(q).getRPY(roll, pitch, yaw); tf::Quaternion qt2 = tf::createQuaternionFromRPY(-rpy[1], -yaw, M_PI); tf::Matrix3x3 R2(qt2); tf::matrixTFToEigen(R2,R); Eigen::Matrix4d tf_final; tf_final.setZero(); tf_final.block (0, 0, 3, 3) << tf_matrix.block (0, 0, 3, 3) * R; //tf_matrix.block (0, 0, 3, 3) = R;//.transpose(); tf_final.block (0, 3, 4, 1) << tf_matrix * tf_matrix_swap * xyz; //tf_matrix (3, 3) = 1; //tf_final = tf_matrix * tf_matrix_swap * tf_final; // Perfrom transformation for (size_t i = 0; i < cloud_in.points.size (); i++) { Eigen::Vector4d pt(cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z, 1); Eigen::Vector4d pt_tf = tf_final*pt; cloud_out.points[i].x = pt_tf[0]; cloud_out.points[i].y = pt_tf[1]; cloud_out.points[i].z = pt_tf[2]; } }
//From Rotation Matrix, find rpy Vector3f rollpy(MatrixXd R, facemidpts facepoints) { float sp, cp; MatrixXf Rf = R.cast<float>(); ros::Rate loop_rate(30); //publish at 30Hz*/ geometry_msgs::Twist posemsg; while(ros::ok()) { if (abs(R(0,0)) < .001 & abs(R(1,0)) < .001) { // singularity rpy(0) = 0; rpy(1) = atan2(-Rf(2,0), Rf(0,0)); rpy(2) = atan2(-Rf(1,2), Rf(1,1)); posemsg.linear.x = facepoints.x; posemsg.linear.y = facepoints.y; posemsg.linear.z = facepoints.z; posemsg.angular.x = rpy(0); posemsg.angular.y = rpy(1); posemsg.angular.z = rpy(2); // ROS_INFO_STREAM("Head Translation: " << posemsg.linear); // ROS_INFO_STREAM("Head RPY Angles: " << posemsg.angular); boost::asio::io_service io_service; const std::string multicast_address = "235.255.0.1"; sender s(io_service, boost::asio::ip::address::from_string(multicast_address), posemsg.linear.x, \ posemsg.linear.y, posemsg.linear.z, posemsg.angular.x, \ posemsg.angular.y, posemsg.angular.z); io_service.run(); loop_rate.sleep(); } else { rpy(0) = atan2(Rf(1,0), Rf(0,0)); sp = sin(rpy(0)); cp = cos(rpy(0)); rpy(1) = atan2(-Rf(2,0), cp * Rf(0,0) + sp * Rf(1,0)); rpy(2) = atan2(sp * Rf(0,2) - cp * Rf(1,2), cp*Rf(1,1) - sp*Rf(0,1)); posemsg.linear.x = facepoints.x; posemsg.linear.y = facepoints.y; posemsg.linear.z = facepoints.z; posemsg.angular.x = rpy(0); posemsg.angular.y = rpy(1); posemsg.angular.z = rpy(2); // ROS_INFO_STREAM("Head Translation: "<< posemsg.linear); // ROS_INFO_STREAM("Head RPY Angles: "<< posemsg.angular); boost::asio::io_service io_service; const std::string multicast_address = "235.255.0.1"; sender s(io_service, boost::asio::ip::address::from_string(multicast_address), posemsg.linear.x, \ posemsg.linear.y, posemsg.linear.z, posemsg.angular.x, \ posemsg.angular.y, posemsg.angular.z); io_service.run(); loop_rate.sleep(); } //ros::waitForShutdown(); } return rpy; }
int main(int argc, char **argv){ ros::init(argc, argv, "automap"); ros::NodeHandle nh; // create dynamic reconfigure object dynamic_reconfigure::Server<automap::ExplorationConfig> server; dynamic_reconfigure::Server<automap::ExplorationConfig>::CallbackType f; automap::ExplorationConfig dynConfig; ros::Time initTime = ros::Time::now(); nav_msgs::MapMetaData mapMetaData; cv::Mat map; simulation::telemetry_msg telemetry; sensor_msgs::ImagePtr edgeImageMsg; MoveBaseClient ac("move_base", true); while(!ac.waitForServer(ros::Duration(5.0)) && ros::ok()) { ROS_INFO("Waiting for the move_base action server to come up..."); } //control_On : motion control on/off / detection_On : sensing on/off / NBV_On : nbv on/off automap::automap_ctrl_msg ctrlSignals; tf::TransformListener listener; geometry_msgs::Pose position; std::vector<double> rpy(3, 0.0); cv::Point gridPose; ros::Subscriber mapMetaSub = nh.subscribe<nav_msgs::MapMetaData>("map_metadata", 10, boost::bind(mapMetaCallback, _1, &mapMetaData)); ros::Subscriber ctrlSub = nh.subscribe<automap::automap_ctrl_msg>("automap/ctrl_msg", 10, boost::bind(ctrlCallback, _1, &ctrlSignals)); ros::Subscriber mapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 10, boost::bind(mapCallback, _1, &map)); ros::Publisher pathPub = nh.advertise<nav_msgs::Path>("pathtransformPlanner/path", 10); ros::Subscriber robotInfo = nh.subscribe<simulation::telemetry_msg>("telemetry", 10, boost::bind(telemetryCallback, _1, &telemetry)); image_transport::ImageTransport it(nh); image_transport::Publisher edgePub = it.advertise("floatingEdges", 1); //wait for map - server ros::Duration d = ros::Duration(2, 0); ros::spinOnce(); while(mapMetaData.resolution == 0 && ros::ok()) { ROS_INFO("Waiting for the map server to come up..."); d.sleep(); ros::spinOnce(); } while(map.cols==0 && map.rows==0 && ros::ok()) { ROS_INFO("Waiting for the map server to come up..."); d.sleep(); ros::spinOnce(); } PathtransformPlanner pPlanner(mapMetaData); ExplorationPlanner ePlanner(&pPlanner); // Begin: exploration metrics // array for drawing the robots path std::vector<cv::Point> explorationPath; // average planner time ros::Time planner_timer; double t_planner = 0; unsigned int t_planner_counter = 0; // average ex-planner time ros::Time explanner_timer; double t_explanner = 0; unsigned int t_explanner_counter = 0; // average busy loop time ros::Time loop_timer; double t_loop = 0; unsigned int t_loop_counter = 0; double timediff = 0; f = boost::bind(&configCallback, _1, _2, &pPlanner, &ePlanner, &dynConfig); server.setCallback(f); ctrlSignals.control_On=dynConfig.node_control_on; ctrlSignals.detection_On=dynConfig.node_sensing_on; ctrlSignals.NBV_On=dynConfig.node_use_nbv; bool finished = false; bool finalCheck = false; int retry = dynConfig.node_retries; cv::Mat old = cv::Mat::zeros(map.rows, map.cols, CV_8UC1); // Loop starts here: ros::Rate loop_rate(dynConfig.node_loop_rate); while(ros::ok() && !finished) { //start measure processing time loop_timer = ros::Time::now(); //calculate information gain double gain = cv::norm(old, map, CV_L2); //Get Position Information... getPositionInfo("map", "base_footprint", listener, &position, &rpy); setGridPosition(position, mapMetaData, &gridPose); //Remember path explorationPath.push_back(gridPose); if(gain>dynConfig.node_information_gain || retry==0) { ROS_INFO("Enough information gained: %lf",gain); old = map.clone(); retry = dynConfig.node_retries; //Feed pathtransformPlanner... try{ ROS_INFO("Feeding PathtransformPlanner..."); //start measure processing time planner_timer = ros::Time::now(); pPlanner.updateTransformMatrices(map, gridPose); //stop measure processing time timediff = (ros::Time::now()-planner_timer).toNSec()/NSEC_PER_SEC; t_planner += timediff; t_planner_counter++; ROS_INFO("Path planning took: %lf",timediff); }catch(std::exception& e) { std::cout<<e.what()<<std::endl; } bool w = false; if(ctrlSignals.detection_On) { //start measure processing time explanner_timer = ros::Time::now(); w = ePlanner.findBestPlan(map, gridPose, rpy[2], ctrlSignals.NBV_On); //stop measure processing time timediff = (ros::Time::now()-explanner_timer).toNSec()/NSEC_PER_SEC; t_explanner += timediff; t_explanner_counter++; ROS_INFO("Exploration planning took: %lf",timediff); if(!w && !finalCheck){ finalCheck = true; continue; } }else{ w = true; } if(w) { finalCheck =false; ROS_INFO("Best next Plan found!"); std_msgs::Header genericHeader; genericHeader.stamp = ros::Time::now(); genericHeader.frame_id = "map"; // send map with valid detected Edges if(dynConfig.node_show_exploration_planner_result){ cv::Mat out = ePlanner.drawFrontiers(); edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg(); edgePub.publish(edgeImageMsg); } nav_msgs::Path frontierPath; if(ctrlSignals.detection_On) { frontierPath = ePlanner.getBestPlan(genericHeader); } if(ctrlSignals.control_On && ctrlSignals.detection_On) { ROS_INFO("Sending Plan.."); pathPub.publish(frontierPath); ros::spinOnce(); ROS_INFO("Sending Goal.."); sendGoal(frontierPath, ac); } }else{ ROS_INFO("Map exploration finished, aborting loop..."); //ac.waitForResult(); ac.cancelGoal(); ac.waitForResult(); std::string imgMetaPath = ros::package::getPath("automap") + "/data/output/map.yaml"; std::string imgStatPath = ros::package::getPath("automap") + "/data/output/exploration_statistics.txt"; std::string imgPath = ros::package::getPath("automap") + "/data/output/map.pgm"; std::string recordedPathPath = ros::package::getPath("automap") + "/data/output/path.png"; //Save explored map std::vector<int> com_param; com_param.push_back(CV_IMWRITE_PNG_COMPRESSION); com_param.push_back(9); try { cv::imwrite(imgPath, map, com_param); ROS_INFO("Map written to: %s", imgPath.c_str()); } catch (std::runtime_error& ex) { std::cout << "Exception converting img to PNG: " << ex.what() << std::endl; } //Save exploration path cv::Mat pathMap = map.clone(); cv::cvtColor(pathMap, pathMap, CV_GRAY2RGB); cv::polylines(pathMap, explorationPath, false, cv::Scalar(0, 0, 255), 1, 8); try { cv::imwrite(recordedPathPath, pathMap, com_param); ROS_INFO("Path written to: %s", recordedPathPath.c_str()); } catch (std::runtime_error& ex) { std::cout << "Exception converting img to PNG: " << ex.what() << std::endl; } YAML::Emitter Y_out; Y_out << YAML::BeginMap; Y_out << YAML::Key << "image"; Y_out << YAML::Value << "map.pgm"; Y_out << YAML::Key << "resolution"; Y_out << YAML::Value << mapMetaData.resolution; Y_out << YAML::Key << "origin"; Y_out << YAML::Flow; Y_out << YAML::BeginSeq << mapMetaData.origin.position.x<<mapMetaData.origin.position.y <<mapMetaData.origin.position.z << YAML::EndSeq; Y_out << YAML::Key << "negate"; Y_out << YAML::Value << 0; Y_out << YAML::Key << "occupied_thresh"; Y_out << YAML::Value << 0.65; Y_out << YAML::Key << "free_thresh"; Y_out << YAML::Value << 0.196; Y_out << YAML::EndMap; YAML::Emitter Y_out_statistics; Y_out_statistics << YAML::BeginMap; Y_out_statistics << YAML::Key << "driven distance during exploration(m)"; Y_out_statistics << YAML::Value << telemetry.radial_distance; Y_out_statistics << YAML::Key << "time elapsed during exploration(s)"; Y_out_statistics << YAML::Value << (ros::Time::now()-initTime).toNSec()/NSEC_PER_SEC; Y_out_statistics << YAML::Key << "average path planner processing time(s)"; Y_out_statistics << YAML::Value << t_planner/t_planner_counter; Y_out_statistics << YAML::Key << "average exploration planner processing time(s)"; Y_out_statistics << YAML::Value << t_explanner/t_explanner_counter; Y_out_statistics << YAML::Key << "average busy loop processing time(s)"; Y_out_statistics << YAML::Value << t_loop/t_loop_counter; // calc quality of map std::string godMapPath = ros::package::getPath("simulation") + "/data/map/map.pgm"; cv::Mat godMap = cv::imread(godMapPath,1); cv::cvtColor(godMap, godMap, CV_RGB2GRAY); double diffMap = cv::norm(godMap, map, CV_L2); Y_out_statistics << YAML::Key << "in comparison to god map (L2Norm)"; Y_out_statistics << YAML::Value << diffMap; Y_out_statistics << YAML::EndMap; std::ofstream outfile_yaml(imgMetaPath); try{ outfile_yaml<<Y_out.c_str(); ROS_INFO("MapMetaData written to: %s", imgMetaPath.c_str()); }catch (std::runtime_error& ex) { std::cout << "Exception writing .yaml-file: " << ex.what() << std::endl; } outfile_yaml.close(); std::ofstream outfile_statistics(imgStatPath); try{ outfile_statistics<<Y_out_statistics.c_str(); ROS_INFO("Statistics data written to: %s", imgStatPath.c_str()); }catch (std::runtime_error& ex) { std::cout << "Exception writing .txt-file: " << ex.what() << std::endl; } outfile_statistics.close(); finished = true; ros::shutdown(); } }else{ ROS_INFO("Not enough information gained: %lf",gain); retry--; } // resend map with valid detected Edges if(dynConfig.node_show_exploration_planner_result){ cv::Mat out = ePlanner.drawFrontiers(); edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg(); edgePub.publish(edgeImageMsg); } //stop measure processing time timediff = (ros::Time::now()-loop_timer).toNSec()/NSEC_PER_SEC; t_loop += timediff; t_loop_counter++; ROS_INFO("Whole loop took: %lf",timediff); ros::spinOnce(); loop_rate.sleep(); } ros::spin(); }
int main(int argc, char** argv){ std::map<std::string,std::string> input; input["-traj_path"] = "/home/guillaume/MatlabWorkSpace/peg_in_hole/TextData/Albert/"; input["-rate"] = "10"; input["-broadcast_plug"] = "plug_link"; input["-broadcast_socket"] = "link_socket"; if(!opti_rviz::Input::process_input(argc,argv,input)){ ROS_ERROR("failed to load input"); return 0; } opti_rviz::Input::print_input_options(input); ros::init(argc, argv, "peg_in_hole_replay_filter"); ros::NodeHandle node; //ros::Rate rate(boost::lexical_cast<float>(input["-rate"])); // initialise world ros::Rate rate(100); /// Wrap the world ww::World_wrapper world_wrapper; geo::fCVec3 origin_ = {{0,0,-0.02/2 - 0.03/2}}; geo::fCVec3 dim_ = {{0.8,0.4,0.05}}; geo::fCVec3 orientation_ = {{M_PI/2,0,M_PI/2}}; wobj::WBox wsocket_wall("socket_wall",dim_,origin_,orientation_); tf::Vector3 origin(0,0,0); tf::Vector3 rpy(M_PI/2,0,M_PI/2); obj::Socket_one socket_one("socket_one",origin,rpy,1); world_wrapper.wrapped_objects.push_back_box(wsocket_wall); world_wrapper.wrapped_objects.push_back_socket(socket_one.wsocket); world_wrapper.wrapped_objects.push_back_box(socket_one.wbox); /// Filter and Sensor parameters Sensor_parameters sensor_parameters(world_wrapper.wrapped_objects); plugfilter::PF_parameters pf_parameters(world_wrapper.wrapped_objects); pf_parameters.particle_filter_type = plugfilter::SIR; pf_parameters.number_particles = 1500; pf_parameters.likelihood_t = likeli::FOUR_CONTACT; sensor_parameters.t_sensor = psm::FOUR_CONTACT_DIST; sensor_parameters.b_print_sensor = true; pf_parameters.visualisation_mode = opti_rviz::Vis_point_cloud::DEFAULT; pf_parameters.pf_color_type = pf::C_WEIGHTS; pf_parameters.sampling_parameters.set_diag(0.0005,0.01,0.01); /// REPLAY Peg_replay peg_replay(node,input["-broadcast_plug"],input["-broadcast_socket"]); peg_replay.set_traj_dir_path(input["-traj_path"]); peg_replay.run_trajectories.setup_pf(pf_parameters); peg_replay.run_trajectories.setup_sensor(sensor_parameters); peg_replay.initalise_vision(node); peg_replay.use_contact_model(world_wrapper.wrapped_objects); peg_replay.run_trajectories.add_reguliser(); /// Visualise features ww::Publisher publisher("visualization_marker",&node,&world_wrapper); publisher.init("/world"); publisher.update_position(); while(node.ok()){ peg_replay.update(); peg_replay.visualise(); publisher.publish(); ros::spinOnce(); rate.sleep(); } return 0; }