int acquireWrapper(bool blocking)
{
const RTIME timeout = blocking ? TM_INFINITE : TM_NONBLOCK;
int ret;
ret = rt_mutex_acquire(&m, timeout);
if (ret == -EPERM) {
// become real-time, then try again
// Allocate a new RT_TASK struct, and then forget the pointer. This
// leak allows us to avoid ownership issues for the RT_TASK, which
// shouldn't necessarily be deleted when the mutex is released, or when
// the mutex is deleted, etc.. It is a small overhead that happens (at
// most) once per thread. If needed, we can always get the pointer back
// by calling rt_task_self().
rt_task_shadow(new RT_TASK, NULL, 10, 0);
ret = rt_mutex_acquire(&m, timeout);
}
if (ret != 0) {
return ret;
}
if (lockCount == 0) {
int oldMode;
ret = rt_task_set_mode(0, T_WARNSW, &oldMode);
if (ret != 0) {
throw std::runtime_error("thread::detail::mutex_impl::acquireWrapper(): Could not set T_WARNSW mode.");
}
leaveWarnSwitchOn = oldMode & T_WARNSW;
}
++lockCount;
return ret;
}
int main(){
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
rt_sem_create(&semaphore, "sem", 1, S_PRIO);
rt_sem_create(&synca, "sync", 0, S_PRIO);
rt_mutex_create(&mutex, "mutex");
RT_TASK L, M, H;
rt_task_shadow(NULL, "main", 4, T_CPU(1)|T_JOINABLE);
rt_task_create(&L, "low", 0, 1, T_CPU(1)|T_JOINABLE);
rt_task_create(&M, "medium", 0, 2, T_CPU(1)|T_JOINABLE);
rt_task_create(&H, "high", 0, 3, T_CPU(1)|T_JOINABLE);
rt_task_start(&L, &low, (void*) 0);
rt_task_start(&M, &medium, (void*) 0);
rt_task_start(&H, &high, (void*) 0);
usleep(100000);
rt_printf("RELEASING SYNC\n");
rt_sem_broadcast(&synca);
rt_task_join(&L);
rt_task_join(&M);
rt_task_join(&H);
rt_sem_delete(&synca);
rt_sem_delete(&semaphore);
rt_mutex_delete(&mutex);
return 0;
}
int main(void)
{
#ifdef XENO_POSIX
struct sched_param sparam;
#else /* __NATIVE_SKIN__ */
RT_TASK main_tid;
#endif /* __NATIVE_SKIN__ */
mlockall(MCL_CURRENT | MCL_FUTURE);
/* Set scheduling parameters for the current process */
#ifdef XENO_POSIX
sparam.sched_priority = 2;
pthread_setschedparam(pthread_self(), SCHED_FIFO, &sparam);
#else /* __NATIVE_SKIN__ */
rt_task_shadow(&main_tid, "main_task", 2, 0);
#endif /* __NATIVE_SKIN__ */
simple_condwait();
relative_condwait();
absolute_condwait();
sig_norestart_condwait();
sig_restart_condwait();
sig_norestart_condwait_mutex();
sig_restart_condwait_mutex();
sig_norestart_double();
sig_restart_double();
cond_destroy_whilewait();
fprintf(stderr, "Test OK\n");
return 0;
}
int main(void)
{
RT_TASK main_tcb;
RT_TASK tcb;
mqd_t mq;
int i;
mlockall(MCL_CURRENT | MCL_FUTURE);
fprintf(stderr, "Checking select service with posix message queues\n");
rt_task_shadow(&main_tcb, NULL, 0, 0);
mq = mq_open("/select_test_mq", O_RDWR | O_CREAT | O_NONBLOCK, 0, NULL);
check_unix(mq == -1 ? -1 : 0);
check_native(rt_task_create(&tcb, "select_test", 0, 1, T_JOINABLE));
check_native(rt_task_start(&tcb, task, (void *)(long)mq));
alarm(30);
for(i = 0; i < sizeof(tunes) / sizeof(tunes[0]); i++) {
check_unix(mq_send(mq, tunes[i], strlen(tunes[i]) + 1, 0));
sleep(1);
}
check_native(rt_task_join(&tcb));
fprintf(stderr, "select service with posix message queues: success\n");
return EXIT_SUCCESS;
}
int main(){
rt_print_auto_init(1);
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_task_shadow(NULL, "main", 5, T_CPU(0)|T_JOINABLE);
#ifdef mutex
rt_mutex_create(&a, "Mutex");
rt_mutex_create(&b, "b");
#endif
rt_task_create(&task1, "Task1", 0, 1, T_CPU(0)|T_JOINABLE);
rt_task_create(&task2, "Task2", 0, 2, T_CPU(0)|T_JOINABLE);
rt_task_start(&task1, &semWait1, NULL);
rt_task_start(&task2, &semWait2, NULL);
rt_printf("sync \n");
rt_task_join(&task1);
rt_task_join(&task2);
#ifdef mutex
rt_mutex_delete(&a);
rt_mutex_delete(&b);
#endif
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
// Xenomai stuff
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "CANServer", 80, 0);
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc !=2 ){
std::cerr << "Usage: " << argv[0] << " can[?]" << std::endl;
return -1;
}
// Better to have the CAN device in loopback mode
std::cout << "Ensure that the CAN device is in loopback mode" << std::endl;
// get local component manager
mtsManagerLocal *taskManager;
taskManager = mtsManagerLocal::GetInstance();
// The RTSocketCAN component
#if (CISST_OS == CISST_LINUX_XENOMAI)
mtsRTSocketCAN can( "CAN",
argv[1],
osaCANBus::RATE_1000,
osaCANBus::LOOPBACK_ON );
#else
mtsSocketCAN can( "CAN",
argv[1],
osaCANBus::RATE_1000,
osaCANBus::LOOPBACK_ON );
#endif
taskManager->AddComponent( &can );
// A client
CANclient client;
taskManager->AddComponent( &client );
// Connect the interfaces
taskManager->Connect( client.GetName(), "IO", can.GetName(), "IO" );
taskManager->Connect( client.GetName(), "CTL", can.GetName(), "CTL" );
taskManager->CreateAll();
taskManager->StartAll();
// Wait to exit
std::cout << "ENTER to exit" << std::endl;
cmnGetChar();
//taskManager->KillAll();
//taskManager->Cleanup();
return 0;
}
int init(int& argc, char** argv, const std::string& name, uint32_t options)
{
ros::init(argc, argv, name, options);
node_name = name;
srand(time(NULL));
signal(SIGINT, rtros_signal_handler);
signal(SIGXCPU, sigxcpu_handler);
rt_print_auto_init(1);
rt_event_create(&recv_data_msg_event, "recv_data_msg_event", 0, EV_PRIO);
return rt_task_shadow(&main_shadow_task, name.c_str(), 0, 0);
}
int main(){
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_print_auto_init(1);
rt_sem_create(&semA, "A", 1, S_FIFO | S_PRIO);
rt_sem_create(&semB, "B", 1, S_FIFO | S_PRIO);
rt_sem_create(&syncsem, "ss", 0, S_FIFO);
RT_TASK tasks[3];
rt_task_create(tasks, "C", 0, 99, T_CPU(0)|T_JOINABLE);
//rt_task_create(&(tasks[1]), "B", 0, 50, T_CPU(0));
rt_task_create(tasks+2, "D", 0, 33, T_CPU(0));
rt_task_start(&(tasks[0]),&task_H,(void*)'H');
//rt_task_start(&(tasks[1]),&task_M,(void*)'M');
rt_task_start(&(tasks[2]),&task_L,(void*)'L');
rt_task_shadow(NULL, "main", 0, 0);
rt_task_sleep_ms(200);
rt_sem_broadcast(&syncsem);
//rt_task_sleep_ms(2000);
rt_task_join(tasks);
rt_task_join(tasks+2);
rt_sem_delete(&semA);
rt_sem_delete(&semB);
/*
pthread_t disturbance[10];
for (i=0; i<10; i++){
pthread_create(&(disturbance[i]),NULL,&busy_wait,NULL);
}
for (i=0; i<10; i++){
pthread_join(disturbance[i],NULL);
}
*/
return 0;
};
int main( int argc, char** argv ){
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " rtcan[0-1]" << std::endl;
return -1;
}
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
if( can.Open() != osaCANBus::ESUCCESS ){
std::cerr << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaBH8_280 BH( &can );
if( BH.Initialize() != osaBH8_280::ESUCCESS ){
std::cerr << "Failed to initialize WAM" << std::endl;
return -1;
}
std::cout << "\n\n\n\n";
double t1 = osaGetTime();
size_t cnt=0;
while( 1 ){
Eigen::VectorXd q( 4, 0.1 );
BH.GetPositions( q );
//BH.SetPositions( q );
}
cmnGetChar();
return 0;
}
int main(void)
{
int err;
RT_TASK task;
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
if ((err = rt_task_shadow(& task, "Hello World 02",
99, T_JOINABLE)) != 0) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
while (1) {
rt_printf("Hello World 02\n");
rt_task_sleep(1000000000LL); // 1 sec.
}
return 0;
}
int main(int argc, char **argv)
{
mlockall(MCL_CURRENT|MCL_FUTURE); //lock current mem alloc and future mem alloc main mem
rt_print_auto_init(1); //hvis vi trenger printf
rt_sem_create(&xenomai_semaphore, "Vaarsemafor", SEM_INIT_COUNT, SEM_MODE);
rt_sem_create(&resource_semaphore, "ressursemafor", SEM_RES_INIT_COUNT, SEM_MODE);
rt_mutex_create(&resMut,"Resource control mutex");
rt_task_create(&taskLow, "task L", AUT0_STCK_SIZE, 50, SINGLE_CORE_MODE);
rt_task_create(&taskMed, "task M", AUT0_STCK_SIZE, 75, SINGLE_CORE_MODE);
rt_task_create(&taskHigh, "task H", AUT0_STCK_SIZE, 99, SINGLE_CORE_MODE);
rt_task_shadow (NULL, "main", 99,SINGLE_CORE_MODE);
rt_task_start(&taskLow, &taskL, NULL);
rt_task_start(&taskMed, &taskM, NULL);
rt_task_start(&taskHigh, &taskH, NULL);
rt_printf("Started program\n");
rt_task_sleep(ONE_SEC);
rt_printf("One second passed\n");
rt_sem_broadcast(&xenomai_semaphore);
while(1){
rt_task_sleep(100000);
}
rt_sem_delete(&xenomai_semaphore);
rt_sem_delete(&resource_semaphore);
rt_mutex_delete(&resMut);
return 0;
}
void makerealtime(char *task_name)
{
int err=0;
mlockall(MCL_CURRENT|MCL_FUTURE); /* Used by Xenomai to avoid paging used RAM.*/
err = rt_task_shadow(&rt_task, task_name, ENGST_PRIORITY, ENGST_CREATE_MODE);
mysyslog(LOG_INFO, "In makerealtime after shadow err = %d\n", err);
/*5 secs ? if fdsched is not running, this blocks forever, why ?*/
err = rt_task_bind(&sched_task,"SCHED_PROXY",5000000000ULL);
if (err < 0 )
{
mysyslog(LOG_ERR, "Bind error %d in %s: %s\n",-err,ext_module_name,strerror(-err));
exit(1);
}
else
mysyslog(LOG_INFO, "Binding engstation to sched task succesfully.\n");
return;
}
void BlockBase::blockSettingsInit(int argc, char *argv[],size_t gssize)
{
mlockall(MCL_CURRENT|MCL_FUTURE);
io.block_name=0;
if(argc!=2){
QString name=argv[0];
name=name.right(name.size()-name.lastIndexOf("/")-1);
qDebug() << QString("Usage: ")+name+" CONFIG_FILE";
errorMessage("blockSettingsInit","No configurations file specified.\n"+
QString("Usage: ")+name+" CONFIG_FILE");
exit(1);
}
rt_task_shadow(&rt_task,NULL,0,0);
if(load_settings(argv[1],gssize)){
QMessageBox::critical(0,"Error",QString("Could not open settings at<BR>")+argv[1]+"<BR>Does file exist?",
QMessageBox::Ok);
exit(1);
}
}
/*!*****************************************************************************
*******************************************************************************
\note initXeno
\date Oct. 2009
\remarks
xenomai specific initializations
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
initXeno(char *task_name)
{
int rc;
struct sigaction sa;
// lock all of the pages currently and pages that become
// mapped into the address space of the process
mlockall(MCL_CURRENT | MCL_FUTURE);
sl_rt_mutex_init(&mutex1);
//become a real-time process
char name[100];
sprintf(name, "x%s_main_%d", task_name, parent_process_id);
rt_task_shadow(NULL, name, 0, 0);
// what to do when mode switches happen
signal(SIGDEBUG, action_upon_switch);
// start the non real-time printing library
rt_print_auto_init(1);
// get the timer info
if ((rc=rt_timer_set_mode((RTIME) XENO_CLOCK_PERIOD)))
printf("rc_timer_set_mode returned %d\n",rc);
// check what we got
RT_TIMER_INFO info;
rt_timer_inquire(&info);
if (info.period == TM_ONESHOT)
printf("Timer Period = TM_ONESHOT\n");
else
printf("Timer Period = %ld [ns]\n",(long) info.period);
}
int main(int argc, char **argv)
{
int i, opt, ret;
struct can_ifreq ifr;
char name[32];
struct option long_options[] = {
{ "help", no_argument, 0, 'h' },
{ "identifier", required_argument, 0, 'i'},
{ "rtr", no_argument, 0, 'r'},
{ "extended", no_argument, 0, 'e'},
{ "verbose", no_argument, 0, 'v'},
{ "count", no_argument, 0, 'c'},
{ "print", required_argument, 0, 'p'},
{ "loop", required_argument, 0, 'l'},
{ "delay", required_argument, 0, 'd'},
{ "send", no_argument, 0, 's'},
{ "timeout", required_argument, 0, 't'},
{ "loopback", required_argument, 0, 'L'},
{ 0, 0, 0, 0},
};
signal(SIGTERM, cleanup_and_exit);
signal(SIGINT, cleanup_and_exit);
frame.can_id = 1;
while ((opt = getopt_long(argc, argv, "hvi:l:red:t:cp:sL:",
long_options, NULL)) != -1) {
switch (opt) {
case 'h':
print_usage(argv[0]);
exit(0);
case 'p':
print = strtoul(optarg, NULL, 0);
case 'v':
verbose = 1;
break;
case 'c':
count = 1;
break;
case 'l':
loops = strtoul(optarg, NULL, 0);
break;
case 'i':
frame.can_id = strtoul(optarg, NULL, 0);
break;
case 'r':
rtr = 1;
break;
case 'e':
extended = 1;
break;
case 'd':
delay = strtoul(optarg, NULL, 0) * 1000000LL;
break;
case 's':
use_send = 1;
break;
case 't':
timeout = strtoul(optarg, NULL, 0) * 1000000LL;
break;
case 'L':
loopback = strtoul(optarg, NULL, 0);
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
break;
}
}
if (optind == argc) {
print_usage(argv[0]);
exit(0);
}
if (argv[optind] == NULL) {
fprintf(stderr, "No Interface supplied\n");
exit(-1);
}
if (verbose)
printf("interface %s\n", argv[optind]);
ret = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
fprintf(stderr, "socket: %s\n", strerror(-ret));
return -1;
}
s = ret;
if (loopback >= 0) {
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
&loopback, sizeof(loopback));
if (ret < 0) {
fprintf(stderr, "setsockopt: %s\n", strerror(-ret));
goto failure;
}
if (verbose)
printf("Using loopback=%d\n", loopback);
}
strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
if (verbose)
printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
ret = ioctl(s, SIOCGIFINDEX, &ifr);
if (ret < 0) {
fprintf(stderr, "ioctl: %s\n", strerror(-ret));
goto failure;
}
memset(&to_addr, 0, sizeof(to_addr));
to_addr.can_ifindex = ifr.ifr_ifindex;
to_addr.can_family = AF_CAN;
if (use_send) {
/* Suppress definiton of a default receive filter list */
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
if (ret < 0) {
fprintf(stderr, "setsockopt: %s\n", strerror(-ret));
goto failure;
}
ret = bind(s, (struct sockaddr *)&to_addr, sizeof(to_addr));
if (ret < 0) {
fprintf(stderr, "bind: %s\n", strerror(-ret));
goto failure;
}
}
if (count)
frame.can_dlc = sizeof(int);
else {
for (i = optind + 1; i < argc; i++) {
frame.data[dlc] = strtoul(argv[i], NULL, 0);
dlc++;
if( dlc == 8 )
break;
}
frame.can_dlc = dlc;
}
if (rtr)
frame.can_id |= CAN_RTR_FLAG;
if (extended)
frame.can_id |= CAN_EFF_FLAG;
if (timeout) {
if (verbose)
printf("Timeout: %lld ns\n", (long long)timeout);
ret = ioctl(s, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
if (ret) {
fprintf(stderr, "ioctl SND_TIMEOUT: %s\n", strerror(-ret));
goto failure;
}
}
snprintf(name, sizeof(name), "rtcansend-%d", getpid());
ret = rt_task_shadow(&rt_task_desc, name, 1, 0);
if (ret) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
goto failure;
}
rt_task();
cleanup();
return 0;
failure:
cleanup();
return -1;
}
int main(int argc, char **argv)
{
char tempname[] = "/tmp/sigdebug-XXXXXX";
char buf[BUFSIZ], dev[BUFSIZ];
RT_TASK main_task, rt_task;
long int start, trash, end;
unsigned char *mayday, *p;
struct sigaction sa;
int old_wd_value;
char r, w, x, s;
int tmp_fd, d;
FILE *maps;
int err;
rt_print_auto_init(1);
if (argc < 2 || strcmp(argv[1], "--skip-watchdog") != 0) {
wd = fopen("/sys/module/xeno_nucleus/parameters/"
"watchdog_timeout", "w+");
if (!wd) {
fprintf(stderr, "FAILURE: no watchdog available and "
"--skip-watchdog not specified\n");
exit(EXIT_FAILURE);
}
err = fscanf(wd, "%d", &old_wd_value);
check("get watchdog", err, 1);
err = fprintf(wd, "2");
check("set watchdog", err, 1);
fflush(wd);
}
maps = fopen("/proc/self/maps", "r");
if (maps == NULL) {
perror("open /proc/self/maps");
exit(EXIT_FAILURE);
}
while (fgets(buf, sizeof(buf), maps)) {
if (sscanf(buf, "%lx-%lx %c%c%c%c %lx %x:%x %d%s\n",
&start, &end, &r, &w, &x, &s, &trash,
&d, &d, &d, dev) == 11
&& r == 'r' && x == 'x'
&& !strcmp(dev, "/dev/rtheap") && end - start == 4096) {
printf("mayday page starting at 0x%lx [%s]\n"
"mayday code:", start, dev);
mayday = (unsigned char *)start;
for (p = mayday; p < mayday + 32; p++)
printf(" %.2x", *p);
printf("\n");
}
}
fclose(maps);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug_handler;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
sa.sa_sigaction = dummy_handler;
sigaction(SIGUSR1, &sa, NULL);
printf("mlockall\n");
munlockall();
setup_checkdebug(SIGDEBUG_NOMLOCK);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check("rt_task_shadow", err, -EINTR);
check_sigdebug_received("SIGDEBUG_NOMLOCK");
mlockall(MCL_CURRENT | MCL_FUTURE);
errno = 0;
tmp_fd = mkstemp(tempname);
check_no_error("mkstemp", -errno);
unlink(tempname);
check_no_error("unlink", -errno);
mem = mmap(NULL, 1, PROT_READ | PROT_WRITE, MAP_SHARED, tmp_fd, 0);
check_no_error("mmap", -errno);
err = write(tmp_fd, "X", 1);
check("write", err, 1);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check_no_error("rt_task_shadow", err);
err = rt_mutex_create(&prio_invert, "prio_invert");
check_no_error("rt_mutex_create", err);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check_no_error("rt_mutex_acquire", err);
err = rt_sem_create(&send_signal, "send_signal", 0, S_PRIO);
check_no_error("rt_sem_create", err);
err = rt_task_spawn(&rt_task, "rt_task", 0, 1, T_WARNSW | T_JOINABLE,
rt_task_body, NULL);
check_no_error("rt_task_spawn", err);
err = rt_sem_p(&send_signal, TM_INFINITE);
check_no_error("rt_sem_signal", err);
pthread_kill(rt_task_thread, SIGUSR1);
rt_task_sleep(rt_timer_ns2ticks(20000000LL));
err = rt_mutex_release(&prio_invert);
check_no_error("rt_mutex_release", err);
err = rt_task_join(&rt_task);
check_no_error("rt_task_join", err);
err = rt_mutex_delete(&prio_invert);
check_no_error("rt_mutex_delete", err);
err = rt_sem_delete(&send_signal);
check_no_error("rt_sem_delete", err);
if (wd) {
fprintf(wd, "%d", old_wd_value);
fclose(wd);
}
fprintf(stderr, "Test OK\n");
return 0;
}
int main( int argc, char** argv ){
// Set up real-time task
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupWAM", 99, 0 );
// Initialize ROS
ros::init(argc, argv, "wam_server", ros::init_options::NoSigintHandler);
// Add custom signal handlers
signal(SIGTERM, quitRequested);
signal(SIGINT, quitRequested);
signal(SIGHUP, quitRequested);
// Construct the wam structure
ros::NodeHandle wam_nh("wam");
barrett_hw::WAM wam_hw( wam_nh );
// Timer variables
struct timespec ts = {0,0};
if(clock_gettime(CLOCK_REALTIME, &ts) != 0) {
ROS_FATAL("Failed to poll realtime clock!");
}
ros::Time
last(ts.tv_sec, ts.tv_nsec),
now(ts.tv_sec, ts.tv_nsec);
ros::Duration period(1.0);
ros::AsyncSpinner spinner(1);
spinner.start();
realtime_tools::RealtimePublisher<std_msgs::Duration> publisher(wam_nh, "loop_rate", 2);
bool wam_ok = false;
while(!g_quit && !wam_ok) {
if(!wam_hw.configure()) {
ROS_ERROR("Could not configure WAM!");
} else if(!wam_hw.start()) {
ROS_ERROR("Could not start WAM!");
} else {
ros::Duration(1.0).sleep();
if(!wam_hw.read(now, period)) {
ROS_ERROR("Could not read from WAM!");
} else {
wam_ok = true;
}
}
ros::Duration(1.0).sleep();
}
// Construct the controller manager
ros::NodeHandle nh;
controller_manager::ControllerManager manager(&wam_hw, nh);
uint32_t count = 0;
// Run as fast as possible
while( !g_quit ) {
// Get the time / period
if (!clock_gettime(CLOCK_REALTIME, &ts)) {
now.sec = ts.tv_sec;
now.nsec = ts.tv_nsec;
period = now - last;
last = now;
} else {
ROS_FATAL("Failed to poll realtime clock!");
break;
}
// Read the state from the WAM
if(!wam_hw.read(now, period)) {
g_quit=true;
break;
}
// Update the controllers
manager.update(now, period);
// Write the command to the WAM
wam_hw.write(now, period);
if(count++ > 1000) {
if(publisher.trylock()) {
count = 0;
publisher.msg_.data = period;
publisher.unlockAndPublish();
}
}
}
publisher.stop();
std::cerr<<"Stpping spinner..."<<std::endl;
spinner.stop();
std::cerr<<"Stopping WAM..."<<std::endl;
wam_hw.stop();
std::cerr<<"Cleaning up WAM..."<<std::endl;
wam_hw.cleanup();
std::cerr<<"Poka!"<<std::endl;
return 0;
}
int main(void){
/* Perform auto-init of rt_print buffers if the task doesn't do so */
rt_print_auto_init(1);
/* Lock memory : avoid memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE);
/* Make main a Real-Time thread */
if(rt_task_shadow(NULL, NULL, RT_MAIN_PRI, T_CPU(CPU_ID) ) != SUCCESS){
printf("RT Thread main failed to be created!\n");
exit(1);
}else{
/* print success */
rt_printf("RT Thread main initiated successfully \n");
}
/* Create semaphore for synchronized start */
if(rt_sem_create(&sync_sem, "sync_start", 0, S_PRIO) != SUCCESS){
rt_printf("sync semaphore failed to be created \n");
exit(1);
}
/* Create Barrier */
#ifdef semaphore
if(rt_sem_create(&sem, "Barrier_sem", 0, S_PRIO) != SUCCESS){
rt_printf("Barrier_sem failed to be created \n");
exit(1);
}
#endif
#ifdef mutex
if(rt_mutex_create(&mut, "Barrier_mut") != SUCCESS){
rt_printf("Barrier_mut failed to be created \n");
exit(1);
}
#endif
/* Initialize Real-Time threads */
int* id;
int i;
RT_TASK rt_threads[N_RT_THREADS];
for(i=0; i<N_RT_THREADS; i++){
/* Give the new thread the channel to respond to by argument */
int *method = malloc(sizeof(int));
*method = USE_METHOD;
/* Create new thread */
if(rt_task_create(&rt_threads[i], NULL, 0, RT_THREADS_PRI[i], T_CPU(CPU_ID)|T_JOINABLE) != SUCCESS){
rt_printf("RT Thread %i failed to be created!\n", i);
exit(1);
}
/* Execute task */
if(rt_task_start(&rt_threads[i], RT_FUNC[i], id) != SUCCESS){
rt_printf("RT Thread %i failed to start!\n", (void*) i);
exit(1);
}
/* print success */
rt_printf("RT Thread %i initiated successfully \n", i);
}
/* Wait untill all threads have been blocked, restart them */
rt_task_sleep(SLEEP_DELAY);
if(rt_sem_broadcast(&sync_sem)!= SUCCESS){
rt_printf("main thread failed to broadcast on BARRIER semaphore \n");
exit(1);
}
rt_task_sleep(SLEEP_DELAY);
/* Wait for all threads to terminate */
for(i=0; i<N_RT_THREADS; i++){
rt_task_join(&rt_threads[i]);
}
/* Clean up */
rt_sem_delete(&sem);
rt_sem_delete(&sync_sem);
rt_mutex_delete(&mut);
return 0;
}
/*!*****************************************************************************
*******************************************************************************
\note checkKeyboard
\date July 7, 1992
\remarks
checks for keyboard interaction an does appropriate functions
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] inital_command : initial command to be executed as it were typed
in the terminal -- pass NULL for nothing
******************************************************************************/
static void *
checkKeyboard(void *initial_command)
{
int result;
long nchars=0;
int i=0;
char prompt[1000];
char *string;
char *ptr, *fptr;
extern double servo_time;
int rc;
#ifdef __XENO__
//become a real-time process
char name[100];
sprintf(name, "%s_terminal_%d", servo_name, parent_process_id);
rt_task_shadow(NULL, name, 0, 0);
// we want this task in non real-time mode
if ((rc=rt_task_set_mode(T_PRIMARY,0,NULL)))
printf("rt_task_set_mode returned %d\n",rc);
#endif
while (run_command_line_thread_flag) {
// run initial command as soon as the task servo has started -- there is
// a little trick that allows resetting the servo clock and re-run the
// initial command, which is useful for simulations.
// the "time_reset_detected" flag is set by the sl_readline_callback
// function which detects a reset of the servo clock
if (time_reset_detected && strcmp(servo_name,"task")==0 && initial_command != NULL ) {
// the clock has been reset
// Special LittleDog Hack -- to be removed?
// if the environment variable "SL_TASK_SERVO_STANDALONE" is set,
// don't wait for the servo time to start ticking:
if (getenv("SL_TASK_SERVO_STANDALONE"))
usleep(100000);
// else wait until the servo_time goes beyond 100ms:
else {
while (servo_time < 0.1)
usleep(10000);
}
if (initial_command != NULL) {
checkUserCommand((char *)initial_command);
}
time_reset_detected = 0;
}
snprintf(prompt, 1000, "%s.%s> ",robot_name,servo_name);
rl_event_hook = &sl_readline_callback;
string = readline(prompt);
if (string && *string)
add_history(string);
checkUserCommand(string);
free(string);
// this allows the user to run a command line command from a program,
// and in partciular a real-time program
if (strlen(user_command) > 0) {
checkUserCommand(user_command);
strcpy(user_command,"\0");
}
}
printf("Command line thread terminated\n");
return NULL;
}
int main(int argc, char *argv[])
{
int ret = 0, len, ofs;
unsigned int i, scan_size = 0, cnt = 0;
unsigned long buf_size;
void *map = NULL;
a4l_desc_t dsc = { .sbdata = NULL };
int (*dump_function) (a4l_desc_t *, a4l_cmd_t*, unsigned char *, int) =
dump_text;
/* Compute arguments */
while ((ret = getopt_long(argc,
argv,
"vrd:s:S:c:mwk:h",
cmd_read_opts, NULL)) >= 0) {
switch (ret) {
case 'v':
verbose = 1;
break;
case 'r':
real_time = 1;
break;
case 'd':
filename = optarg;
break;
case 's':
cmd.idx_subd = strtoul(optarg, NULL, 0);
break;
case 'S':
cmd.stop_arg = strtoul(optarg, NULL, 0);
break;
case 'c':
str_chans = optarg;
break;
case 'm':
use_mmap = 1;
break;
case 'w':
dump_function = dump_raw;
break;
case 'k':
wake_count = strtoul(optarg, NULL, 0);
break;
case 'h':
default:
do_print_usage();
return 0;
}
}
if (isatty(STDOUT_FILENO) && dump_function == dump_raw) {
fprintf(stderr,
"cmd_read: cannot dump raw data on a terminal\n\n");
return -EINVAL;
}
/* Recover the channels to compute */
do {
cmd.nb_chan++;
len = strlen(str_chans);
ofs = strcspn(str_chans, ",");
if (sscanf(str_chans, "%u", &chans[cmd.nb_chan - 1]) == 0) {
fprintf(stderr, "cmd_read: bad channel argument\n");
return -EINVAL;
}
str_chans += ofs + 1;
} while (len != ofs);
/* Update the command structure */
cmd.scan_end_arg = cmd.nb_chan;
cmd.stop_src = cmd.stop_arg != 0 ? TRIG_COUNT : TRIG_NONE;
if (real_time != 0) {
if (verbose != 0)
printf("cmd_read: switching to real-time mode\n");
/* Prevent any memory-swapping for this program */
ret = mlockall(MCL_CURRENT | MCL_FUTURE);
if (ret < 0) {
ret = errno;
fprintf(stderr, "cmd_read: mlockall failed (ret=%d)\n",
ret);
goto out_main;
}
/* Turn the current process into an RT task */
ret = rt_task_shadow(&rt_task_desc, NULL, 1, 0);
if (ret < 0) {
fprintf(stderr,
"cmd_read: rt_task_shadow failed (ret=%d)\n",
ret);
goto out_main;
}
}
/* Open the device */
ret = a4l_open(&dsc, filename);
if (ret < 0) {
fprintf(stderr, "cmd_read: a4l_open %s failed (ret=%d)\n",
filename, ret);
return ret;
}
if (verbose != 0) {
printf("cmd_read: device %s opened (fd=%d)\n",
filename, dsc.fd);
printf("cmd_read: basic descriptor retrieved\n");
printf("\t subdevices count = %d\n", dsc.nb_subd);
printf("\t read subdevice index = %d\n", dsc.idx_read_subd);
printf("\t write subdevice index = %d\n", dsc.idx_write_subd);
}
/* Allocate a buffer so as to get more info (subd, chan, rng) */
dsc.sbdata = malloc(dsc.sbsize);
if (dsc.sbdata == NULL) {
fprintf(stderr, "cmd_read: malloc failed \n");
return -ENOMEM;
}
/* Get this data */
ret = a4l_fill_desc(&dsc);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_fill_desc failed (ret=%d)\n", ret);
goto out_main;
}
if (verbose != 0)
printf("cmd_read: complex descriptor retrieved\n");
/* Get the size of a single acquisition */
for (i = 0; i < cmd.nb_chan; i++) {
a4l_chinfo_t *info;
ret = a4l_get_chinfo(&dsc,
cmd.idx_subd, cmd.chan_descs[i], &info);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_get_chinfo failed (ret=%d)\n",
ret);
goto out_main;
}
if (verbose != 0) {
printf("cmd_read: channel %x\n", cmd.chan_descs[i]);
printf("\t ranges count = %d\n", info->nb_rng);
printf("\t bit width = %d (bits)\n", info->nb_bits);
}
scan_size += a4l_sizeof_chan(info);
}
if (verbose != 0) {
printf("cmd_read: scan size = %u\n", scan_size);
if (cmd.stop_arg != 0)
printf("cmd_read: size to read = %u\n",
scan_size * cmd.stop_arg);
}
/* Cancel any former command which might be in progress */
a4l_snd_cancel(&dsc, cmd.idx_subd);
if (use_mmap != 0) {
/* Get the buffer size to map */
ret = a4l_get_bufsize(&dsc, cmd.idx_subd, &buf_size);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_get_bufsize() failed (ret=%d)\n",
ret);
goto out_main;
}
if (verbose != 0)
printf("cmd_read: buffer size = %lu bytes\n", buf_size);
/* Map the analog input subdevice buffer */
ret = a4l_mmap(&dsc, cmd.idx_subd, buf_size, &map);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_mmap() failed (ret=%d)\n",
ret);
goto out_main;
}
if (verbose != 0)
printf
("cmd_read: mmap performed successfully (map=0x%p)\n",
map);
}
ret = a4l_set_wakesize(&dsc, wake_count);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_set_wakesize failed (ret=%d)\n", ret);
goto out_main;
}
if (verbose != 0)
printf("cmd_read: wake size successfully set (%lu)\n",
wake_count);
/* Send the command to the input device */
ret = a4l_snd_command(&dsc, &cmd);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_snd_command failed (ret=%d)\n", ret);
goto out_main;
}
if (verbose != 0)
printf("cmd_read: command successfully sent\n");
if (use_mmap == 0) {
/* Fetch data */
do {
/* Perform the read operation */
ret = a4l_async_read(&dsc, buf, BUF_SIZE, A4L_INFINITE);
if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_read failed (ret=%d)\n",
ret);
goto out_main;
}
/* Display the results */
if (dump_function(&dsc, &cmd, buf, ret) < 0) {
ret = -EIO;
goto out_main;
}
/* Update the counter */
cnt += ret;
} while (ret > 0);
} else {
unsigned long front = 0;
/* Fetch data without any memcpy */
do {
/* Retrieve and update the buffer's state
(In input case, we recover how many bytes are available
to read) */
ret = a4l_mark_bufrw(&dsc, cmd.idx_subd, front, &front);
if (ret == -ENOENT)
break;
else if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_mark_bufrw() failed (ret=%d)\n",
ret);
goto out_main;
}
/* If there is nothing to read, wait for an event
(Note that a4l_poll() also retrieves the data amount
to read; in our case it is useless as we have to update
the data read counter) */
if (front == 0) {
ret = a4l_poll(&dsc, cmd.idx_subd, A4L_INFINITE);
if (ret == 0)
break;
else if (ret < 0) {
fprintf(stderr,
"cmd_read: a4l_poll() failed (ret=%d)\n",
ret);
goto out_main;
}
}
/* Display the results */
if (dump_function(&dsc,
&cmd,
&((unsigned char *)map)[cnt % buf_size],
front) < 0) {
ret = -EIO;
goto out_main;
}
/* Update the counter */
cnt += front;
} while (1);
}
if (verbose != 0)
printf("cmd_read: %d bytes successfully received\n", cnt);
ret = 0;
out_main:
if (use_mmap != 0)
/* Clean the pages table */
munmap(map, buf_size);
/* Free the buffer used as device descriptor */
if (dsc.sbdata != NULL)
free(dsc.sbdata);
/* Release the file descriptor */
a4l_close(&dsc);
return ret;
}
int main(int argc, char **argv)
{
int opt, ret;
u_int32_t id, mask;
u_int32_t err_mask = 0;
struct ifreq ifr;
char *ptr;
char name[32];
struct option long_options[] = {
{ "help", no_argument, 0, 'h' },
{ "verbose", no_argument, 0, 'v'},
{ "filter", required_argument, 0, 'f'},
{ "error", required_argument, 0, 'e'},
{ "timeout", required_argument, 0, 't'},
{ "timestamp", no_argument, 0, 'T'},
{ "timestamp-rel", no_argument, 0, 'R'},
{ 0, 0, 0, 0},
};
mlockall(MCL_CURRENT | MCL_FUTURE);
signal(SIGTERM, cleanup_and_exit);
signal(SIGINT, cleanup_and_exit);
while ((opt = getopt_long(argc, argv, "hve:f:t:p:RT",
long_options, NULL)) != -1) {
switch (opt) {
case 'h':
print_usage(argv[0]);
exit(0);
case 'p':
print = strtoul(optarg, NULL, 0);
break;
case 'v':
verbose = 1;
break;
case 'e':
err_mask = strtoul(optarg, NULL, 0);
break;
case 'f':
ptr = optarg;
while (1) {
id = strtoul(ptr, NULL, 0);
ptr = strchr(ptr, ':');
if (!ptr) {
fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
exit(1);
}
ptr++;
mask = strtoul(ptr, NULL, 0);
ptr = strchr(ptr, ':');
add_filter(id, mask);
if (!ptr)
break;
ptr++;
}
break;
case 't':
timeout = (nanosecs_rel_t)strtoul(optarg, NULL, 0) * 1000000;
break;
case 'R':
timestamp_rel = 1;
case 'T':
with_timestamp = 1;
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
break;
}
}
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret));
return -1;
}
s = ret;
if (argv[optind] == NULL) {
if (verbose)
printf("interface all\n");
ifr.ifr_ifindex = 0;
} else {
if (verbose)
printf("interface %s\n", argv[optind]);
strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
if (verbose)
printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr);
if (ret < 0) {
fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", strerror(-ret));
goto failure;
}
}
if (err_mask) {
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
&err_mask, sizeof(err_mask));
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
if (verbose)
printf("Using err_mask=%#x\n", err_mask);
}
if (filter_count) {
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER,
&recv_filter, filter_count *
sizeof(struct can_filter));
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
}
recv_addr.can_family = AF_CAN;
recv_addr.can_ifindex = ifr.ifr_ifindex;
ret = rt_dev_bind(s, (struct sockaddr *)&recv_addr,
sizeof(struct sockaddr_can));
if (ret < 0) {
fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret));
goto failure;
}
if (timeout) {
if (verbose)
printf("Timeout: %lld ns\n", (long long)timeout);
ret = rt_dev_ioctl(s, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
if (ret) {
fprintf(stderr, "rt_dev_ioctl RCV_TIMEOUT: %s\n", strerror(-ret));
goto failure;
}
}
if (with_timestamp) {
ret = rt_dev_ioctl(s, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS);
if (ret) {
fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", strerror(-ret));
goto failure;
}
}
snprintf(name, sizeof(name), "rtcanrecv-%d", getpid());
ret = rt_task_shadow(&rt_task_desc, name, 0, 0);
if (ret) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
goto failure;
}
rt_task();
/* never returns */
failure:
cleanup();
return -1;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "mtsWAMGCExample", 60, 0 );
#endif
mtsTaskManager* taskManager = mtsTaskManager::GetInstance();
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
mtsKeyboard kb;
kb.SetQuitKey( 'q' );
kb.AddKeyWriteFunction( 'C', "GCEnable", "Enable", true );
taskManager->AddComponent( &kb );
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 );
WAM.Configure();
WAM.SetPositions( vctDynamicVector<double>(7,
0.0, -cmnPI_2, 0.0, cmnPI,
0.0, -cmnPI_2, 0.0 ) );
taskManager->AddComponent( &WAM );
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
mtsGravityCompensation GC( "GC",
0.002,
path.Find( "wam7.rob" ),
Rtw0,
OSA_CPU3 );
taskManager->AddComponent( &GC );
if( !taskManager->Connect( kb.GetName(), "GCEnable",
GC.GetName(), "Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::GCEnable to "
<< GC.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( WAM.GetName(), "Input",
GC.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Input to "
<< GC.GetName() << "::Output" << std::endl;
return -1;
}
if( !taskManager->Connect( WAM.GetName(), "Output",
GC.GetName(), "Input" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Output to "
<< GC.GetName() << "::Input" << std::endl;
return -1;
}
taskManager->CreateAll();
taskManager->StartAll();
pause();
return 0;
}
/**
* BORDERLINE THREAD
*/
void ImuInterfaceNonRT::communicationLoop()
{
/**
* Initialization
*/
#ifdef __XENO__
rt_task_shadow(NULL, "BorderlineImuCom", 0, 0);
// immediately go into secondary mode:
// rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
bool stop_reading = stop_reading_;
try
{
imu_.openPort("/dev/ttyACM0");
}
catch(...)
{
return;
}
std::cout << "The port to the imu sensor has been opened." << std::endl;
std::cout << "Firmware Version Number: " << imu_.getFirmwareNumber() << std::endl;
double fix_off = 0;
try
{
imu_.initTime(fix_off);
}
catch(...)
{
return;
}
try
{
imu_.initGyros();
}
catch(...)
{
return;
}
uint64_t time;
uint32_t timestamp;
double accel[3];
double unstab_accel[3];
double angrate[3];
// double mag[3];
double orientation[9];
double stab_accel[/*3*/] = {0.0, 0.0, 0.0};
double dummy_angrate[3];
double stab_mag[3];
// double delta_angle[3];
// double delta_velocity[3];
SL_quat sl_orient;
SL_Cstate sl_pos, sl_pos_unstab;
try
{
// imu_.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_MAG_ORIENT);
}
catch(...)
{
return;
}
std::cout << "Initial Timestamp: " << imu_.initTimestamp() << std::endl;
std::cout << "IMU: finished initialization, entering loop..." << std::endl;
// start_time_ = 1000000.0*std::clock()/CLOCKS_PER_SEC;
#ifdef __XENO__
rt_task_set_periodic(NULL, TM_NOW, 1000000*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM);
#endif
while(!stop_reading)
{
// std::cout << "microseconds passed: " << 1000.0*std::clock()/CLOCKS_PER_SEC - start_time_ << std::endl;
// usleep(100000 - (int)(1000.0*std::clock()/CLOCKS_PER_SEC) % 100000);
for(int i=0; i<3; i++)
{
accel[i] = 0.0;
unstab_accel[i] = 0.0;
}
imu_.transactAccelAngrateOrientation(&time, unstab_accel, angrate, orientation, ×tamp);
imu_.transactStabAccAngrateStabMag(&time, stab_accel, dummy_angrate, stab_mag);
// imu_.transactDeltaAngleDeltaVel(&time, delta_angle, delta_velocity);
for(int i=0; i<3; i++)
{
accel[i] = unstab_accel[i] - stab_accel[i];
// delta_velocity[i] -= stab_accel[i] *0.001*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM;
}
imuDataToSLQuat(accel, angrate, orientation, sl_orient, sl_pos, unstab_accel, sl_pos_unstab);
int reads = 0;
int writes = 0;
// const double leackage = 0.99;
/**
* PRIMARY MODE
*/
{
SauronsRing the_ring(mutex_);
for(unsigned int i=0; i<9; i++)
raw_rot_mat_[i] = orientation[i];
for(unsigned int i=0; i<3; i++)
{
raw_ang_vel_[i] = angrate[i];
raw_acc_[i] = accel[i];
raw_unstab_acc_[i] = unstab_accel[i];
// raw_delta_linvel_[i] *= leackage;
// raw_delta_linvel_[i] += delta_velocity[i];
// raw_delta_angle_[i] += delta_angle[i];
// sl_pos.xd[i+1] += leackage*position_.xd[i+1];
}
position_ = sl_pos;
position_unstab_ = sl_pos_unstab;
orientation_ = sl_orient;
timestamp_ = time;
stop_reading = stop_reading_;
num_writes_++;
reads = num_reads_;
writes = num_writes_;
initialized_ = true;
}
// std::cout << "left primary mode..." << std::endl;
#ifdef __XENO__
rt_task_wait_period(NULL);
#endif
};
// communication stopped
imu_.closePort();
std::cout << "leaving communication loop." << std::endl;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "mtsWAMPDGCExample", 40, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 3 ){
std::cout << "Usage: " << argv[0] << " can[0-1] GCMIP" << std::endl;
return -1;
}
std::string process( "Slave" );
mtsManagerLocal* taskManager = NULL;
try{ taskManager = mtsTaskManager::GetInstance( argv[2], process ); }
catch( ... ){
std::cerr << "Failed to connect to GCM: " << argv[2] << std::endl;
taskManager = mtsManagerLocal::GetInstance();
}
mtsKeyboard kb;
kb.SetQuitKey( 'q' );
kb.AddKeyWriteFunction( 'C', "PDGCEnable", "Enable", true );
kb.AddKeyWriteFunction( 'C', "TrajEnable", "Enable", true );
kb.AddKeyWriteFunction( 'M', "MoveEnable", "Enable", true );
taskManager->AddComponent( &kb );
// Initial configuration
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI-0.01;
qinit[5] = -cmnPI_2;
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 );
WAM.Configure();
WAM.SetPositions( qinit );
taskManager->AddComponent( &WAM );
cmnPath path;
path.AddRelativeToCisstShare("models/WAM");
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
mtsPDGC PDGC( "PDGC",
0.00125,
path.Find( "wam7cutter.rob" ),
Rtw0,
Kp,
Kd,
qinit,
OSA_CPU3 );
taskManager->AddComponent( &PDGC );
mtsTrajectory traj( "trajectory",
0.002,
path.Find( "wam7cutter.rob" ),
Rtw0,
qinit );
taskManager->AddComponent( &traj );
SetPoints setpoints( path.Find( "wam7cutter.rob" ), Rtw0, qinit );
taskManager->AddComponent( &setpoints );
// Connect the keyboard
if( !taskManager->Connect( kb.GetName(), "PDGCEnable",
PDGC.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::PDGCEnable to "
<< PDGC.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "TrajEnable",
traj.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< traj.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "MoveEnable",
setpoints.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< setpoints.GetName() << "::Control" << std::endl;
return -1;
}
// connect the trajectory with setpoints
if( !taskManager->Connect( traj.GetName(), "Input",
setpoints.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Input to "
<< setpoints.GetName() << "::Output" << std::endl;
return -1;
}
// connect the trajectory to the controller
if( !taskManager->Connect( traj.GetName(), "Output",
PDGC.GetName(), "Input") ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Output to "
<< PDGC.GetName() << "::Input" << std::endl;
return -1;
}
// connect the controller to the WAM
if( !taskManager->Connect( WAM.GetName(), "Input",
PDGC.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Input to "
<< PDGC.GetName() << "::Output" << std::endl;
return -1;
}
if( !taskManager->Connect( WAM.GetName(), "Output",
PDGC.GetName(), "Feedback" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Output to "
<< PDGC.GetName() << "::Feedback" << std::endl;
return -1;
}
taskManager->CreateAll();
taskManager->StartAll();
std::cout << "Press 'q' to exit." << std::endl;
pause();
return 0;
}
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main)
{
// first check if root (or if have sufficient privileges)
if ( geteuid() != 0 ) {
#if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2302
printf( "WARNING: You are not root. This program *may* require that you are root.\n");
// \todo verify have sufficient privileges
#else
printf( "You are not root. This program requires that you are root.\n");
exit(1);
#endif
}
// locking of all memory for this process
int rv = mlockall(MCL_CURRENT | MCL_FUTURE);
if ( rv != 0 ) {
perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable.
exit(1);
}
struct sched_param param;
// we set the MT to the highest sched priority to allow the console
// to interrupt a loose running thread.
param.sched_priority = sched_get_priority_max(ORO_SCHED_OTHER);
if (param.sched_priority != -1 )
sched_setscheduler( 0, ORO_SCHED_OTHER, ¶m);
const char* mt_name = "MainThread";
main->sched_type = SCHED_XENOMAI_SOFT; // default for MainThread
main->name = strncpy( (char*)malloc( (strlen(mt_name)+1)*sizeof(char) ), mt_name, strlen(mt_name)+1 );
int ret = -1;
while( ret != 0) {
// name, priority, mode
if ( (ret = rt_task_shadow( &(main->xenotask),mt_name, 0, 0)) != 0 ) {
if ( ret == -ENOMEM ) {
// fail: abort
printf( "Cannot rt_task_create() MainThread: Out of memory.\n");
exit(1);
}
if ( ret == -EBUSY ) {
// ok: we are a xeno thread (may log() ):
log(Info) << "MainThread already a Xenomai task." <<endlog();
break;
}
if ( ret == -EEXIST ) {
// fail: retry without using a name.
mt_name = 0; // do not register
continue;
}
if ( ret == -EPERM ) {
// fail: abort
printf( "Can not rt_task_create() MainThread: No permission.\n");
exit(1);
}
// uncaught error: abort
printf( "Can not rt_task_create() MainThread: Error %d.\n",ret);
exit(1);
}
}
// We are a xeno thread now:
// Only use Logger after this point (i.e. when rt_task_shadow was succesful).
if ( mt_name == 0) {
log(Warning) << "'MainThread' name was already in use. Registered empty name with Xenomai.\n" <<endlog();
}
// main is created in main thread.
main->xenoptr = rt_task_self();
#ifdef OROSEM_OS_XENO_PERIODIC
# if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
// time in nanoseconds
rt_timer_start( ORODAT_OS_XENO_PERIODIC_TICK*1000*1000*1000 );
Logger::In in("Scheduler");
Logger::log() << Logger::Info << "Xenomai Periodic Timer started using "<<ORODAT_OS_XENO_PERIODIC_TICK<<" seconds." << Logger::endl;
# else
Logger::In in("Scheduler");
Logger::log() << Logger::Error << "Set Xenomai Periodic Timer using the Linux kernel configuration." << Logger::endl;
# endif
#else
# if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
rt_timer_start( TM_ONESHOT );
Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
# else
# if CONFIG_XENO_OPT_TIMING_PERIODIC
Logger::log() << Logger::Info << "Xenomai Periodic Timer configured in 'periodic' mode." << Logger::endl;
# else
Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl;
# endif
# endif
#endif
log(Info) << "Installing SIGXCPU handler." <<endlog();
//signal(SIGXCPU, warn_upon_switch);
struct sigaction sa;
sa.sa_handler = warn_upon_switch;
sigemptyset( &sa.sa_mask );
sa.sa_flags = 0;
sigaction(SIGXCPU, &sa, 0);
Logger::log() << Logger::Debug << "Xenomai Timer and Main Task Created" << Logger::endl;
return 0;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaWAM WAM( &can );
if( WAM.Initialize() != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl;
return -1;
}
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI;
if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl;
return -1;
}
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob", cmnPath::READ);
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit );
std::cout << "Activate the WAM" << std::endl;
bool activated = false;
double t1 = osaGetTime();
while( 1 ){
// Get the positions
vctDynamicVector<double> q;
if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl;
return -1;
}
// Check if the pucks are activated
if( !activated ) {
osaWAM::Mode mode;
if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl;
return -1;
}
if( mode == osaWAM::MODE_ACTIVATED )
{ activated = true; }
}
// if pucks are activated, run the controller
vctDynamicVector<double> tau( q.size(), 0.0 );
double t2 = osaGetTime();
if( activated ){
if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl;
return -1;
}
}
t1 = t2;
// apply torques
if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl;
return -1;
}
std::cout << "q: " << q << std::endl;
std::cout << "tau: " << tau << std::endl;
}
return 0;
}
