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);
}
