int main(int argc, char **argv)
{
int i, msg_count;
legged_robot::Encoder encoder_msg;
moveVelocity = SATURATION_POS;
desiredPos = 0;
msg_count = 0;
InitErrors();
ros::init(argc, argv, "FL_motor_pid");
ros::NodeHandle n;
ros::Publisher motor_pid_pub = n.advertise<legged_robot::PWM>("FL_pwm_feedback", 1000);
ros::Subscriber motor_interface_sub = n.subscribe("FL_encoder_feedback", 1000, encoderCallback);
ros::Subscriber motor_position_sub = n.subscribe("FL_Position", 1000, positionCallback);
ros::Rate loop_rate(12000);
ROS_INFO("Staring communication with FL Motor Controller node.");
while (ros::ok())
{
if(sendMsg)
{
sendMsg = false;
motor_pid_pub.publish(pwm_msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
// Main Function
int main(int argc, char **argv)
{
int i;
nodes::LegEncoders encoders_msg;
moveVelocity_hip = moveVelocity_knee = SATURATION_POS;
desiredPosHip = desiredPosKnee = 1000;
msg_count = 0;
InitErrors();
// Initialize ROS node
ros::init(argc, argv, "FL_leg_control");
ros::NodeHandle n;
// Initialize the publisher for command data post
ros::Publisher motor_pid_pub = n.advertise<nodes::LegCommand>("FL_command_dispatch", 1000);
//ros::Publisher test_pub = n.advertise<nodes::LegCommand>("testCallback", 1000);
// Initialize the subscribers for Encoder data reception and desired position reception
ros::Subscriber motor_interface_sub = n.subscribe("FL_encoders_feedback", 1000, encoderCallback);
ros::Subscriber motor_position_sub = n.subscribe("FL_Position", 1000, positionCallback);
ros::Rate loop_rate(12);
ROS_INFO("Staring communication with FL leg interface node.");
while (ros::ok())
{
//if(msg_count == 100)
//{
msg_count = 0;
ROS_INFO("ENC : %d\n",tenc[0]);
//}
// If a PWM command has been calculated publish it to the topic
if(sendMsg)
{
sendMsg = false;
motor_pid_pub.publish(command_msg);
//test_pub.publish(command_msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
IrcErrorHandler::IrcErrorHandler (QObject *parent)
: QObject (parent)
{
InitErrors ();
}
int
main(int argc, char *argv[])
{
int i, oldumask;
argcGlobal = argc;
argvGlobal = argv;
configfilename = NULL;
/* init stuff */
ProcessCmdLine(argc, argv);
/*
* Do this first thing, to get any options that only take effect at
* startup time. It is read again each time the server resets.
*/
if (ReadConfigFile(configfilename) != FSSuccess) {
FatalError("couldn't read config file\n");
}
InitErrors();
/* make sure at least world write access is disabled */
if (((oldumask = umask(022)) & 002) == 002)
(void)umask(oldumask);
SetDaemonState();
SetUserId();
while (1) {
serverGeneration++;
OsInit();
if (serverGeneration == 1) {
/* do first time init */
CreateSockets(OldListenCount, OldListen);
InitProcVectors();
clients = (ClientPtr *) fsalloc(MAXCLIENTS * sizeof(ClientPtr));
if (!clients)
FatalError("couldn't create client array\n");
for (i = MINCLIENT; i < MAXCLIENTS; i++)
clients[i] = NullClient;
/* make serverClient */
serverClient = (ClientPtr) fsalloc(sizeof(ClientRec));
if (!serverClient)
FatalError("couldn't create server client\n");
}
ResetSockets();
/* init per-cycle stuff */
InitClient(serverClient, SERVER_CLIENT, (pointer) 0);
clients[SERVER_CLIENT] = serverClient;
currentMaxClients = MINCLIENT;
currentClient = serverClient;
if (!InitClientResources(serverClient))
FatalError("couldn't init server resources\n");
InitAtoms();
InitFonts();
SetConfigValues();
if (!create_connection_block())
FatalError("couldn't create connection block\n");
#ifdef DEBUG
fprintf(stderr, "Entering Dispatch loop\n");
#endif
Dispatch();
#ifdef DEBUG
fprintf(stderr, "Leaving Dispatch loop\n");
#endif
/* clean up per-cycle stuff */
if ((dispatchException & DE_TERMINATE) || drone_server)
break;
fsfree(ConnectionInfo);
/* note that we're parsing it again, for each time the server resets */
if (ReadConfigFile(configfilename) != FSSuccess)
FatalError("couldn't read config file\n");
}
CloseSockets();
CloseErrors();
exit(0);
}
int
CloneMyself(void)
{
int child;
char old_listen_arg[256];
char *arg_ptr = old_listen_arg;
int i, j;
int lastfdesc;
char portnum[20];
assert(!drone_server); /* a drone shouldn't hit this */
if (!CloneSelf)
return -1;
#ifdef __UNIXOS2__
NoticeF("cloning of font server not supported under OS/2!\n");
return(-1);
#endif
old_listen_arg[0] = '\0';
lastfdesc = sysconf(_SC_OPEN_MAX) - 1;
if ( (lastfdesc < 0) || (lastfdesc > MAXSOCKS)) {
lastfdesc = MAXSOCKS;
}
NoticeF("attempting clone...\n");
chdir("/");
child = fork();
if (child == -1) {
/* failed to fork */
ErrorF("clone failed to fork()\n");
return -1;
}
/*
* Note: they still share the same process group, and killing the parent
* will take out all the kids as well. this is considered a feature (at
* least until i'm convinced otherwise)
*/
if (child == 0) {
StopListening();
NoticeF("clone: child becoming drone\n");
drone_server = TRUE;
return 1;
} else { /* parent */
NoticeF("clone: parent revitalizing as %s\n", progname);
CloseErrors();
/* XXX should we close stdio as well? */
for (i = 3; i < lastfdesc; i++)
{
for (j = 0; j < ListenTransCount; j++)
if (ListenTransFds[j] == i)
break;
if (j >= ListenTransCount)
(void) close(i);
}
for (i = 0; i < ListenTransCount; i++)
{
int trans_id, fd;
char *port;
if (!_FontTransGetReopenInfo (ListenTransConns[i],
&trans_id, &fd, &port))
continue;
sprintf (arg_ptr, "%d/%d/%s", trans_id, fd, port);
arg_ptr += strlen (arg_ptr);
free (port);
if (i < ListenTransCount - 1)
{
strcat (arg_ptr, ",");
arg_ptr++;
}
}
sprintf (portnum, "%d", ListenPort);
if (*old_listen_arg != '\0')
execlp(progname, progname,
"-ls", old_listen_arg,
"-cf", configfilename,
"-port", portnum,
(void *)NULL);
InitErrors(); /* reopen errors, since we don't want to lose
* this */
Error("clone failed");
FatalError("failed to clone self\n");
}
/* NOTREACHED */
return 0;
}
