long long pn(long long n, long long x) { long long s; if(n==0) s=1; else if(n==1) s=x; else s=((2*n-1)*x*pn(n-1,x)-(n-1)*pn(n-2,x))/n; return s; }
void vis_brd::_drw_fld(int i, int j, bool hlght) { assert(0 <= i && 0 <= j); assert(i < snpsht_.get_wdth() && j < snpsht_.get_hght()); plr_clr p = snpsht_.get_cell(i + j * snpsht_.get_wdth()); static const int spc = fld_sz / 8; QPainter pnt(this); /** * Step 1: Draw the bounding rectangle. */ if (hlght == false) { QPen pn(Qt::black); pn.setWidth(wdt); pnt.setBrush(QColor(250, 250, 200)); pnt.setPen(pn); } else { QPen pn(Qt::red); pn.setWidth(wdt); pnt.setBrush(QColor(250, 250, 225)); pnt.setPen(pn); } int fr_x = i * fld_sz + wdt / 2; int fr_y = j * fld_sz + wdt / 2; pnt.drawRect(fr_x, fr_y, fld_sz, fld_sz); /** * Step 2: Draw the circle. */ { QPen pn(Qt::black); pn.setWidth(wdt); if (p == pc_wht) pnt.setBrush(QBrush(Qt::red)); else if (p == pc_blc) pnt.setBrush(QBrush(Qt::black)); pnt.setPen(pn); } if (p != pc_free) pnt.drawEllipse(fr_x + spc, fr_y + spc, fld_sz - 2 * spc, fld_sz - 2 * spc); }
static void pn (const te_expr *n, int depth) { printf("%*s", depth, ""); if (n->bound) { printf("bound %p\n", n->bound); } else if (n->left == 0 && n->right == 0) { printf("%f\n", n->value); } else if (n->left && n->right == 0) { printf("f1 %p\n", n->left); pn(n->left, depth+1); } else { printf("f2 %p %p\n", n->left, n->right); pn(n->left, depth+1); pn(n->right, depth+1); } }
int main(int argc, char** argv) { ros::init(argc, argv, "USB2_F_7001_node"); ros::NodeHandle n; ros::NodeHandle pn("~"); rx_pub = n.advertise<can_msgs::CANFrame>("/can_bus_rx", 10); ros::Subscriber tx_sub = n.subscribe<can_msgs::CANFrame>("/can_bus_tx", 10, CANFrameToSend); std::string port; //pn.param<std::string>("port", port, "/dev/ttyUSB0"); pn.param<std::string>("port", port, "/dev/serial/by-id/usb-LAWICEL_CANUSB_LWVPMVC4-if00-port0"); int bit_rate; pn.param("bit_rate", bit_rate, 5); can_usb_adapter = new USB2_F_7001(&port, &CANFrameReceived); ROS_INFO("USB2_F_7001 -- Opening CAN bus..."); if( !can_usb_adapter->openCANBus((USB2_F_7001_BitRate)bit_rate) ) { ROS_FATAL("USB2_F_7001 -- Failed to open the CAN bus!"); ROS_BREAK(); } ROS_INFO("USB2_F_7001 -- The CAN bus is now open!"); ros::spin(); delete can_usb_adapter; return(0); }
int main(int argc, char** argv) { ros::init(argc, argv, "kinect_simulated_tilt"); ros::NodeHandle n; ros::NodeHandle pn("~"); ros::Duration(0.5).sleep(); ros::Publisher tilt_pub = n.advertise<std_msgs::Float64>("cur_tilt_angle", 10); ros::Subscriber sub_tilt_angle = n.subscribe("tilt_angle", 10, setTiltAngle); ros::Rate loop_rate(30); while (ros::ok()) { // tilt angle std_msgs::Float64 angle_msg; angle_msg.data = pitch * 180.0 / M_PI; //send the joint state and transform tilt_pub.publish(angle_msg); ros::spinOnce(); // This will adjust as needed per iteration loop_rate.sleep(); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "roomba_tf_setup"); ros::NodeHandle n; ros::NodeHandle pn("~"); std::string base_frame_id; std::string laser_frame_id; std::string nose_frame_id; pn.param<std::string>("base_frame_id", base_frame_id, "base_link"); pn.param<std::string>("laser_frame_id", laser_frame_id, "base_laser"); pn.param<std::string>("nose_frame_id", nose_frame_id, "base_nose"); ros::Rate r(50); tf::TransformBroadcaster broadcaster; while(n.ok()) { broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.065, 0.000, 0.240)), ros::Time::now(), base_frame_id.c_str(), laser_frame_id.c_str())); broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.120, 0.000, 0.160)), ros::Time::now(), base_frame_id.c_str(), nose_frame_id.c_str())); r.sleep(); } }
SickTimCommon::SickTimCommon(AbstractParser* parser) : diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser) // FIXME All Tims have 15Hz? { dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f; f = boost::bind(&sick_tim::SickTimCommon::update_config, this, _1, _2); dynamic_reconfigure_server_.setCallback(f); // datagram publisher (only for debug) ros::NodeHandle pn("~"); pn.param<bool>("publish_datagram", publish_datagram_, false); if (publish_datagram_) datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000); // scan publisher pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000); diagnostics_.setHardwareID("none"); // set from device after connection diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(pub_, diagnostics_, // frequency should be target +- 10%. diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10), // timestamp delta can be from 0.0 to 1.3x what it ideally is. diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_)); ROS_ASSERT(diagnosticPub_ != NULL); }
void QDeclarativeRectangle::generateRoundedRect() { Q_D(QDeclarativeRectangle); if (d->rectImage.isNull()) { const int pw = d->pen && d->pen->isValid() ? d->pen->width() : 0; const int radius = qCeil(d->radius); //ensure odd numbered width/height so we get 1-pixel center QString key = QLatin1String("q_") % QString::number(pw) % d->color.name() % QString::number(d->color.alpha(), 16) % QLatin1Char('_') % QString::number(radius); if (d->pen && d->pen->isValid()) key += d->pen->color().name() % QString::number(d->pen->color().alpha(), 16); if (!QPixmapCache::find(key, &d->rectImage)) { d->rectImage = QPixmap(radius*2 + 3 + pw*2, radius*2 + 3 + pw*2); d->rectImage.fill(Qt::transparent); QPainter p(&(d->rectImage)); p.setRenderHint(QPainter::Antialiasing); if (d->pen && d->pen->isValid()) { QPen pn(QColor(d->pen->color()), d->pen->width()); p.setPen(pn); } else { p.setPen(Qt::NoPen); } p.setBrush(d->color); if (pw%2) p.drawRoundedRect(QRectF(qreal(pw)/2+1, qreal(pw)/2+1, d->rectImage.width()-(pw+1), d->rectImage.height()-(pw+1)), d->radius, d->radius); else p.drawRoundedRect(QRectF(qreal(pw)/2, qreal(pw)/2, d->rectImage.width()-pw, d->rectImage.height()-pw), d->radius, d->radius); // end painting before inserting pixmap // to pixmap cache to avoid a deep copy p.end(); QPixmapCache::insert(key, d->rectImage); } } }
/** * Prints an integer value * @param input The unsigned value. */ void pn(unsigned int input) { int ldb = input % 10; int n = (int) input / 10; if (input > 9) pn(n); printChar(ldb + 48); }
JointTrajectoryGenerator(std::string name) : as_(ros::NodeHandle(), "joint_trajectory_generator", boost::bind(&JointTrajectoryGenerator::executeCb, this, _1), false), ac_("joint_trajectory_action"), got_state_(false) { ros::NodeHandle n; state_sub_ = n.subscribe("state", 1, &JointTrajectoryGenerator::jointStateCb, this); ros::NodeHandle pn("~"); pn.param("max_acc", max_acc_, 0.5); pn.param("max_vel", max_vel_, 5.0); // Load Robot Model ROS_DEBUG("Loading robot model"); std::string xml_string; ros::NodeHandle nh_toplevel; if (!nh_toplevel.getParam(std::string("/robot_description"), xml_string)) throw ros::Exception("Could not find paramter robot_description on parameter server"); if(!robot_model_.initString(xml_string)) throw ros::Exception("Could not parse robot model"); ros::Rate r(10.0); while(!got_state_){ ros::spinOnce(); r.sleep(); } ac_.waitForServer(); as_.start(); ROS_INFO("%s: Initialized",name.c_str()); }
int main(int argc, char *argv[]) { ros::init(argc, argv, "kml_extractor_node"); ros::NodeHandle n; ros::NodeHandle pn("~"); signal(SIGINT, sigintHandler); std::string header_file_path, footer_file_path; pn.param<std::string>("kml_file", kml_filename, "out.kml"); pn.param<std::string>("coordinates_filename", coordinates_utm_filename, "coordinates_utm.txt"); pn.param<std::string>("header_file_path", header_file_path, "/home/joao/catkin_ws_isrobotcar/src/kml_extractor/src/header.yaml"); pn.param<std::string>("footer_file_path", footer_file_path, "/home/joao/catkin_ws_isrobotcar/src/kml_extractor/src/footer.yaml"); kml_file.open(kml_filename.c_str(), std::ios_base::out | std::ios_base::trunc); coordinates_utm_file.open(coordinates_utm_filename.c_str(), std::ios_base::out | std::ios_base::trunc); std::ifstream header_kml(header_file_path.c_str()); footer_kml.open(footer_file_path.c_str(), std::ios_base::in); while(header_kml.good()) { char c = header_kml.get(); // get character from file if (header_kml.good()) kml_file << c; } kml_file << std::endl; ros::Subscriber fix_sub = n.subscribe("fix", 30, callbackFix); ros::Subscriber odom_sub = n.subscribe("odom", 30, callbackOdom); ros::spin(); return 0; }
db_err udb_server::find_dbr_cb(const char *key_str, const char *pn_str, http_response *rsp) { std::string key(key_str); std::string pn(pn_str); db_record *dbr = seeks_proxy::_user_db->find_dbr(key,pn); if (!dbr) { return DB_ERR_NO_REC; } else { // serialize dbr. std::string str; dbr->serialize(str); // fill up response. size_t body_size = str.length() * sizeof(char); if (!rsp->_body) rsp->_body = (char*)std::malloc(body_size); rsp->_content_length = body_size; for (size_t i=0; i<str.length(); i++) rsp->_body[i] = str[i]; delete dbr; return SP_ERR_OK; } }
int main (int argc, char **argv) { ros::init (argc, argv, "pgr_camera"); typedef dynamic_reconfigure::Server < pgr_camera_driver::PGRCameraConfig > Server; Server server; try { boost::shared_ptr < PGRCameraNode > pn (new PGRCameraNode (ros::NodeHandle(),ros::NodeHandle("~"))); Server::CallbackType f = boost::bind (&PGRCameraNode::configure, pn, _1, _2); server.setCallback (f); ros::spin (); } catch (std::runtime_error & e) { ROS_FATAL ("Uncaught exception: '%s', aborting.", e.what ()); ROS_BREAK (); } return 0; }
rowvec dF2du(unsigned row, irowvec causes, const DataPairs &data, const gmat &sigma, vec u){ // Attaining variance covariance matrix etc. (conditional on u) vmat cond_sig = sigma(causes); vec cond_mean = cond_sig.proj*u; rowvec alp = data.alpha_get(row, causes); rowvec gam = data.gamma_get(row, causes); colvec alpgam = alp.t() - gam.t(); double cdf = pn(alpgam, cond_mean, cond_sig.vcov); vec ll = sqrt(diagvec(cond_sig.vcov)); mat Lambda = diagmat(ll); mat iLambda = diagmat(1/ll); mat R = iLambda*cond_sig.vcov*iLambda; mat LR = Lambda*R; double r = R(0,1); vec ytilde = iLambda*(alpgam - cond_mean); vecmat D = Dbvn(ytilde(0),ytilde(1),r); mat M = -LR*D.V; vec dcdfdu = trans(cond_sig.proj)*cond_sig.inv*M; rowvec dF2du_1 = data.dpiduMarg_get(row, causes(0), 0)*data.piMarg_get(row, causes(1), 1)*cdf ; rowvec dF2du_2 = data.dpiduMarg_get(row, causes(1), 1)*data.piMarg_get(row, causes(0), 0)*cdf; vec dF2du_3 = data.piMarg_get(row, causes(0), 0)*data.piMarg_get(row, causes(1), 1)*dcdfdu; rowvec dF2du = dF2du_1 + dF2du_2 + dF2du_3.t(); return(dF2du); };
TEST_F(PubNubCppTest, PubNubInitDone) { { PubNub pn("demo", "demo", &cb, NULL); EXPECT_TRUE(initCalled); } EXPECT_TRUE(doneCalled); }
int main(int argc, char **argv) { ros::init(argc, argv, "sweep_node"); ros::NodeHandle n; ros::NodeHandle pn("~"); double min; pn.param("min", min, -0.8); double max; pn.param("max", max, 0.8); double height; pn.param("height", height, 0.05); double speed; pn.param("speed", speed, 0.2); double acceleration; pn.param("acceleration", acceleration, 0.5); actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("/arm_controller/follow_joint_trajectory", true); ROS_INFO("Sweep -- Waiting for the action server to start..."); ac.waitForServer(); ROS_INFO("Sweep -- Got it!"); double position = min; while(ros::ok()) { control_msgs::FollowJointTrajectoryGoal goal; goal.trajectory.header.stamp = ros::Time::now(); goal.trajectory.joint_names.resize(2); goal.trajectory.points.resize(1); goal.trajectory.joint_names[0] = "upper_arm_joint"; goal.trajectory.points[0].positions.push_back(height); goal.trajectory.points[0].velocities.push_back(speed); goal.trajectory.points[0].accelerations.push_back(acceleration); goal.trajectory.joint_names[1] = "arm_axel_joint"; goal.trajectory.points[0].positions.push_back(position = position == min ? max : min); goal.trajectory.points[0].velocities.push_back(speed); goal.trajectory.points[0].accelerations.push_back(acceleration); goal.trajectory.points[0].time_from_start = ros::Duration(2.0); goal.goal_tolerance.resize(4); goal.goal_tolerance[0].name = "upper_arm_joint"; goal.goal_tolerance[0].position = 0.01; goal.goal_tolerance[1].name = "arm_axel_joint"; goal.goal_tolerance[1].position = 0.01; goal.goal_time_tolerance = ros::Duration(0.5); ac.sendGoal(goal); // Wait for the action to return bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); if(!finished_before_timeout) { actionlib::SimpleClientGoalState state = ac.getState(); ROS_ERROR("Sweep -- %s!", state.toString().c_str()); } } return 0; }
/* Conditional on other individual */ double F1(unsigned row, unsigned cause, unsigned indiv, unsigned cond_cause, const DataPairs &data, const gmat &sigma, vec u){ // Other individual unsigned cond_indiv; if (indiv==0){ cond_indiv=1; } else { cond_indiv=0; } // Alpgam of other individual double cond_alp = data.alphaMarg_get(row, cond_cause, cond_indiv); double cond_gam = data.gammaMarg_get(row, cond_cause, cond_indiv); double cond_alpgam = cond_alp - cond_gam; vec vcond_alpgam(1); vcond_alpgam(0) = cond_alpgam; // Joining u vector and alpgam from other individual vec alpgamu = join_cols(vcond_alpgam, u); // Attaining variance covariance matrix etc. (conditional on u and other individual) vmat cond_sig = sigma(cause,cond_cause); double cond_mean = as_scalar(cond_sig.proj*alpgamu); double alp = data.alphaMarg_get(row, cause, indiv); double gam = data.gammaMarg_get(row, cause, indiv); double alpgam = alp - gam; double F1 = data.piMarg_get(row, cause, indiv)*pn(alpgam, cond_mean, as_scalar(cond_sig.vcov)); return(F1); };
void TerrainMeshSource::generateNoise(int x, int y, int w, int h, int numOctaves, float amplitude, float frequency) { if(w == -1) w = getHeightmapWidth(); if(h == -1) h = getHeightmapHeight(); RECT rc; rc.left = x; rc.right = x+w; rc.top = y; rc.bottom = y+h; clipRect(rc); // int cw = rc.right - rc.left; int ch = rc.bottom - rc.top; if(cw <=0 || ch <= 0) return; Perlin pn(numOctaves, frequency, amplitude, timeGetTime()); if(m_mapBits == 16) copyNoiseToHeightmap(pn, rc, &m_16bitHMap, 65535); else copyNoiseToHeightmap(pn, rc, &m_8bitHMap, 255); }
static Value * eval(const Ast *expr) { switch(expr->class) { case N_CALL: return call(expr); case N_ASSIGNMENT: return assignment(expr); case N_IDENTIFIER: return identifier(expr); case N_NEG: return _neg(expr); case N_NOT: return _not(expr); case N_EQ: return _eq(expr); case N_NEQ: return _neq(expr); case N_AND: return _and(expr); case N_IOR: return _ior(expr); case N_XOR: return _neq(expr); // alias case N_LT: return _lt(expr); case N_LE: return _le(expr); case N_GE: return _ge(expr); case N_GT: return _gt(expr); case N_ADD: return _add(expr); case N_SUB: return _sub(expr); case N_MUL: return _mul(expr); case N_DIV: return _div(expr); case N_POW: return _pow(expr); case N_MOD: return _mod(expr); case N_BOOLEAN: return _bool(expr); case N_INTEGER: return _int(expr); case N_FLOAT: return _float(expr); case N_STRING: return _string(expr); case N_SET: return _set(expr); case N_R: return _relation(expr); } printf("EVALFAIL %d ", expr->class); pn(expr); assert(false && "should not be reached"); }
bool FishEyeCamera::gen_ray(double dx, double dy, Vector3D &ray_dir) { Vector3D u = 1.0 * m_properties->m_left_dir; Vector3D v = 1.0 * m_properties->m_up_dir; Vector3D w = -1.0 * m_properties->m_view_dir; u.normalize(); v.normalize(); w.normalize(); Point2D pn(m_properties->m_pixel_size * 2.0 / (m_properties->m_pixel_size * m_properties->m_screen_width) * dx, m_properties->m_pixel_size * 2.0 / (m_properties->m_pixel_size * m_properties->m_screen_height) * dy); double r_squared = pn[0] * pn[0] + pn[1] * pn[1]; if (r_squared <= 1.0) { float r = sqrt(r_squared); float psi = r * m_psi_max * PI_ON_180; float sin_psi = sin(psi); float cos_psi = cos(psi); float sin_alpha = pn[1] / r; float cos_alpha = pn[0] / r; ray_dir = sin_psi * cos_alpha * u + sin_psi * sin_alpha * v - cos_psi * w; return true; } return false; }
// ----------------------------------------------------------------------------- ray direction // explained on page 192 Vector3D Spherical::ray_direction( const Point2D& pp, const int hres, const int vres, const float s ) const { // compute the normalised device coordinates Point2D pn( 2.0 / (s * hres) * pp.x, 2.0 / (s * vres) * pp.y); // compute the angles lambda and phi in radians float lambda = pn.x * lambda_max * PI_ON_180; float psi = pn.y * psi_max * PI_ON_180; // compute the regular azimuth and polar angles float phi = PI - lambda; float theta = 0.5 * PI - psi; float sin_phi = sin(phi); float cos_phi = cos(phi); float sin_theta = sin(theta); float cos_theta = cos(theta); // equation 11.6 Vector3D dir = sin_theta * sin_phi * u + cos_theta * v + sin_theta * cos_phi * w; return (dir); }
TEST_F(PubNubCppTest, PubNubAutoDone) { pubnub *p = pubnub_init("demo", "demo", &cb, NULL); { PubNub pn(p, true); EXPECT_FALSE(initCalled); } EXPECT_TRUE(doneCalled); }
void Column::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { painter->setBrush(QBrush(Qt::red)); QPen pn(Qt::red); pn.setWidth(0); painter->setPen(pn); painter->drawRect(boundingRect()); }
/// @{ conversion functions inline Point2 pixelToNormalized( const Point2& p ) const { BOOST_STATIC_ASSERT( boost::is_floating_point<F>::value ); BOOST_STATIC_ASSERT( ( boost::is_same<F, double>::value ) ); Point2 pn( p / _imgHalfDiagonal ); pn.x *= _pixelRatio; return pn; }
void print(Node *o) { if(o==null) return; print(o->son[0]); // fprintf(fout,"%d ",o->val); pn(o->val);pc(' '); print(o->son[1]); }
int main(int argc, char** argv) { ros::init(argc, argv, "TeresaNode"); ros::NodeHandle n; ros::NodeHandle pn("~"); Teresa::NodeCalib node_calib(n,pn); return 0; }
int main(int argc, char ** argv) { ros::init(argc, argv, "rtk_base_station"); ROS_INFO("RTKlib for ROS Base Station Edition"); ros::NodeHandle n; ros::NodeHandle pn("~"); std::string port; pn.param<std::string>("port", port, "/dev/ttyACM1"); int baudrate; pn.param("baudrate", baudrate, 115200); cereal::CerealPort serial_gps; try{ serial_gps.open(port.c_str(), baudrate); } catch(cereal::Exception& e) { ROS_FATAL("RTK -- Failed to open the serial port!!!"); ROS_BREAK(); } char buffer[350]; int count; buffer[108]=0; buffer[0]='l'; buffer[1]='s'; buffer[2]='e'; buffer[3]=1; ros::Publisher pub = n.advertise<std_msgs::ByteMultiArray>("base_station/gps/raw_data", 100); ROS_INFO("RTK -- Streaming data..."); while(ros::ok()) { try{ count = serial_gps.read(buffer, 300, 1000); } catch(cereal::TimeoutException& e) { ROS_WARN("RTK -- Failed to get data from GPS!"); } std_msgs::ByteMultiArray raw_msg; for(int i=0 ; i<count ; i++) { raw_msg.data.push_back(buffer[i]); } pub.publish(raw_msg); ros::Duration(0.1).sleep(); } return 0; }
TEST_F(PubNubCppTest, PubNubNotAutoDone) { pubnub *p = pubnub_init("demo", "demo", &cb, NULL); { PubNub pn(p); EXPECT_FALSE(initCalled); } ASSERT_FALSE(doneCalled); pubnub_done(p); }
bool AABB::InsideOf(const Plane& n) const{ vec3 h = (Max - Min)*.5f; vec3 pn(fabs(n.a), fabs(n.b), fabs(n.c)); float e = Dot(h, pn); float s = Dot(GetCenter(), n) + n.d; if(s-e > 0) return true; if(s+e < 0) return false; return true; }
static void exec(const Ast *ast) { //printf("executing "); pn(ast); Value *v; switch(ast->class) { case N_BLOCK: block(ast); return; case N_IF: ifstat(ast); return; case N_WHILE: whilestat(ast); return; case N_DECLARATION: declaration(ast); return; case N_FUNCTION: return; case N_RETURN: returnstat(ast); return; case N_CALL: case N_ASSIGNMENT: case N_IDENTIFIER: case N_NEG: case N_NOT: case N_EQ: case N_NEQ: case N_AND: case N_IOR: case N_XOR: case N_LT: case N_LE: case N_GE: case N_GT: case N_ADD: case N_SUB: case N_MUL: case N_DIV: case N_POW: case N_MOD: case N_BOOLEAN: case N_INTEGER: case N_FLOAT: case N_STRING: case N_SET: case N_R: v = eval(ast); //do { print_tree(2, ast); dprintf(2, " evaluates to "); pv(v); } while(0); value_free(v); return; } printf("EXECFAIL %d ", ast->class); pn(ast); assert(false && "should not be reached"); }