void AllegroNode::updateController() { // Calculate loop time; tnow = ros::Time::now(); dt = 1e-9 * (tnow - tstart).nsec; tstart = tnow; // save last iteration info for (int i = 0; i < DOF_JOINTS; i++) { previous_position[i] = current_position[i]; previous_position_filtered[i] = current_position_filtered[i]; previous_velocity[i] = current_velocity[i]; } updateWriteReadCAN(); // Low-pass filtering. for (int i = 0; i < DOF_JOINTS; i++) { current_position_filtered[i] = (0.6 * current_position_filtered[i]) + (0.198 * previous_position[i]) + (0.198 * current_position[i]); current_velocity[i] = (current_position_filtered[i] - previous_position_filtered[i]) / dt; current_velocity_filtered[i] = (0.6 * current_velocity_filtered[i]) + (0.198 * previous_velocity[i]) + (0.198 * current_velocity[i]); current_velocity[i] = (current_position[i] - previous_position[i]) / dt; } computeDesiredTorque(); publishData(); frame++; }
virtual void SetUp() { // generate a pointcloud generateTestData(); // setup ROS ros::NodeHandle n("~"); pub_cloud_ = n.advertise<sensor_msgs::PointCloud2>("output", 1); pub_even_indices_ = n.advertise<pcl_msgs::PointIndices>("output/even_indices", 1); pub_odd_indices_ = n.advertise<pcl_msgs::PointIndices>("output/odd_indices", 1); sub_even_result_ = n.subscribe("input/even_result", 1, &ExtractIndicesTest::evenCallback, this); sub_odd_result_ = n.subscribe("input/odd_result", 1, &ExtractIndicesTest::oddCallback, this); sub_even_organized_result_ = n.subscribe("input/even_organized_result", 1, &ExtractIndicesTest::evenOrganizedCallback, this); sub_odd_organized_result_ = n.subscribe("input/odd_organized_result", 1, &ExtractIndicesTest::oddOrganizedCallback, this); // wait until ros::Time start_time = ros::Time::now(); while (!isReady()) { publishData(); ros::spinOnce(); ros::Duration(0.5).sleep(); } ros::Time end_time = ros::Time::now(); ROS_INFO("took %f sec to retrieve result", (end_time - start_time).toSec()); }
void ObstacleDetector::interpret() { if (debug){ cvNamedWindow("Control Box", 1); } int s=5; if (debug) { cvCreateTrackbar("Kernel 1", "Control Box", &s, 20); cv::namedWindow("Raw Scan", 0); cv::imshow("Raw Scan", img); cv::waitKey(WAIT_TIME); } int dilation_size = EXPAND_OBS; cv::Mat element = cv::getStructuringElement( cv::MORPH_ELLIPSE, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ); cv::dilate(img, img, element); if (debug) { cv::namedWindow("Dilate Filter", 1); cv::imshow("Dilate Filter", img); cv::waitKey(WAIT_TIME); } publishData(); }
/** * * @brief Main function * * @param [in] int * @param [in] char * * */ int main(int argc, char *argv[]) { ROS_INFO("Starting argos3d_p100 ros..."); ros::init (argc, argv, "argos3d_p100"); ros::NodeHandle nh; dynamic_reconfigure::Server<argos3d_p100::argos3d_p100Config> srv; dynamic_reconfigure::Server<argos3d_p100::argos3d_p100Config>::CallbackType f; f = boost::bind(&callback, _1, _2); if(initialize(argc, argv,nh)){ first = true; srv.setCallback(f); first = false; ROS_INFO("Initalized Camera... Reading Data"); ros::Rate loop_rate(10); while (nh.ok() && dataPublished) { if(publishData()) dataPublished==true; ros::spinOnce (); loop_rate.sleep (); } } else { ROS_WARN("Cannot Initialize Camera. Check the parameters and try again!!"); return 0; } pmdClose (hnd); return 0; }
bool IMUPosition::update(ros::Duration dt) { ros::Time currentTime = ros::Time::now(); stateEstimatePosition(currentTime); publishData(dt, currentTime); return true; }
void LocalPlanner::plan() { ros::Rate loop_rate(LOOP_RATE); navigation::AStarSeed planner(std::string("")); while (ros::ok()) { ros::spinOnce(); std::pair<std::vector<navigation::StateOfCar>, navigation::Seed> path = planner.findPathToTargetWithAstar(local_map ,my_bot_location, my_target_location); planner.showPath(path.first); publishData(path); loop_rate.sleep(); } }
void ctkExampleDicomAppLogic::onCreateSecondaryCapture() { const QPixmap* pixmap = ui.PlaceHolderForImage->pixmap(); if(pixmap!=NULL) { QStringList preferredProtocols; preferredProtocols.append("file:"); QString outputlocation = getHostInterface()->getOutputLocation(preferredProtocols); QString templatefilename = QDir(outputlocation).absolutePath(); if(templatefilename.isEmpty()==false) templatefilename.append('/'); templatefilename.append("ctkdahscXXXXXX.jpg"); QTemporaryFile *tempfile = new QTemporaryFile(templatefilename,this->AppWidget); if(tempfile->open()) { QString filename = QFileInfo(tempfile->fileName()).absoluteFilePath(); qDebug() << "Created file: " << filename; tempfile->close(); QPixmap tmppixmap(*pixmap); QPainter painter(&tmppixmap); painter.setPen(Qt::white); painter.setFont(QFont("Arial", 15)); painter.drawText(tmppixmap.rect(),Qt::AlignBottom|Qt::AlignLeft,"Secondary capture by ctkExampleDicomApp"); //painter.drawText(rect(), Qt::AlignCenter, "Qt"); tmppixmap.save(tempfile->fileName(), "JPEG"); qDebug() << "Created Uuid: " << getHostInterface()->generateUID(); ctkDicomAppHosting::AvailableData resultData; ctkDicomAvailableDataHelper::addToAvailableData(resultData, objectLocatorCache(), tempfile->fileName()); bool success = publishData(resultData, true); if(!success) { qCritical() << "Failed to publish data"; } qDebug() << " publishData returned: " << success; } else qDebug() << "Creating temporary file failed."; } }
void ObstacleDetector::interpret() { if (debug_mode > 0) { cv::imshow(std::string("/") + node_name + std::string("/raw_scan"), obstacle_map); cv::waitKey(wait_time); } int dilation_size = obstacle_expansion; cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), cv::Point(dilation_size, dilation_size)); cv::dilate(obstacle_map, obstacle_map, element); publishData(); if (debug_mode > 0) { cv::imshow(std::string("/") + node_name + std::string("/dilate_filter"), obstacle_map); cv::waitKey(wait_time); } }
void ICACHE_FLASH_ATTR mqttConnectedCb(uint32_t *args) { char topic[100]; MQTT_Client* client = (MQTT_Client*) args; mqttConnected = true; INFOP("MQTT is Connected to %s:%d\n", sysCfg.mqtt_host, sysCfg.mqtt_port); os_sprintf(topic, "/Raw/%s/set/#", sysCfg.device_id); INFOP("Subscribe to: %s\n", topic); MQTT_Subscribe(client, topic, 0); os_sprintf(topic, "/Raw/%s/+/set/filter", sysCfg.device_id); INFOP("Subscribe to: %s\n", topic); MQTT_Subscribe(client, topic, 0); MQTT_Subscribe(client, "/App/#", 0); publishDeviceInfo(client); publishData(client); os_timer_disarm(&switch_timer); os_timer_setfn(&switch_timer, (os_timer_func_t *) switchTimerCb, NULL); os_timer_arm(&switch_timer, 100, true); os_timer_disarm(&display_timer); os_timer_setfn(&display_timer, (os_timer_func_t *) displayCb, NULL); os_timer_arm(&display_timer, 2000, true); os_timer_disarm(&date_timer); os_timer_setfn(&date_timer, (os_timer_func_t *) dateTimerCb, NULL); os_timer_arm(&date_timer, 10 * 60 * 1000, false); //10 minutes os_timer_disarm(&transmit_timer); os_timer_setfn(&transmit_timer, (os_timer_func_t *) transmitCb, (void *) client); os_timer_arm(&transmit_timer, sysCfg.updates * 1000, true); lightOff(); }
/** * * @brief Main function * * @param [in] int * @param [in] char * * */ int main(int argc, char *argv[]) { std::cout << "---------------------------------------------------\n"; std::cout << "| Starting ARGOS3D - P100 |\n"; std::cout << "---------------------------------------------------\n"; std::cout << "- Initalizion of camera"<< "\n"; //TimeMeasurement time; if(initialize()){ std::cout << "- Camera initialized => Reading data"<< "\n"; while (dataPublished) { //time.start(); if(!publishData()) break; //time.end(); } } else { std::cout << "- Cannot Initialize Camera :\n \t Check the parameters and try again!! \n"; std::cout << "---------------------------------------------------\n"; return 0; } std::cout << "---------------------------------------------------\n"; BTAfreeFrame(&frame); BTAclose(&btaHandle); return 0; }
//virtual method from threadClass void* ddsDataWriter::run() { publishData(); return 0; }
void Socket::publishData(const uint8_t* buf, size_t len, const ndn::time::milliseconds& freshness, const Name& prefix) { publishData(ndn::dataBlock(ndn::tlv::Content, buf, len), freshness, prefix); }
void ICACHE_FLASH_ATTR transmitCb(uint32_t *args) { MQTT_Client* client = (MQTT_Client*) args; publishData(client); }