const void *start_mock_server(char **cmdline) {
struct mock_server_info *info = calloc(1, sizeof(*info));
if (info == NULL) {
return NULL;
}
if (!create_monitor(info)) {
free(info);
return NULL;
}
info->pid = fork();
assert(info->pid != -1);
if (info->pid == 0) {
/* Child */
char monitor[1024];
char *argv[1024];
int arg = 0;
argv[arg++] = (char*)"./tests/start_mock.sh";
sprintf(monitor, "--harakiri-monitor=localhost:%d", info->port);
argv[arg++] = monitor;
if (cmdline != NULL) {
int ii = 0;
while (cmdline[ii] != NULL && arg < 1022) {
argv[arg++] = cmdline[ii++];
}
}
argv[arg++] = NULL;
execv(argv[0], argv);
abort();
} else {
char buffer[1024];
ssize_t offset;
ssize_t nr;
/* wait until the server connects */
info->client = accept(info->sock, NULL, NULL);
assert(info->client != -1);
/* Get the port number of the http server */
offset = snprintf(buffer, sizeof(buffer), "localhost:");
nr = recv(info->client, buffer + offset,
sizeof(buffer) - (size_t)offset - 1, 0);
assert(nr > 0);
buffer[nr + offset] = '\0';
info->http = strdup(buffer);
wait_for_server(buffer + offset);
}
return info;
}
PlanToAction()
{
motion_source_= NO_GRASP;
ros::NodeHandle nh("");
// Trajectory Controllers
l_arm_traj_pub_ = nh.advertise<trajectory_msgs::JointTrajectory>("l_arm_trajectory",1,false);
r_arm_traj_pub_ = nh.advertise<trajectory_msgs::JointTrajectory>("r_arm_trajectory",1,false);
torso_traj_pub_ = nh.advertise<trajectory_msgs::JointTrajectory>("torso_trajectory",1,false);
neck_traj_pub_ = nh.advertise<trajectory_msgs::JointTrajectory>("neck_trajectory",1,false);
// Connection to planning service
/*
pose_planning_srv_client_ = nh.serviceClient<vigir_teleop_planning_msgs::GetMotionPlanForPose>("get_plan");
joints_planning_srv_client_ = nh.serviceClient<vigir_teleop_planning_msgs::GetMotionPlanForJoints>("get_plan_joints");
circular_planning_srv_client_ = nh.serviceClient<vigir_teleop_planning_msgs::GetMotionPlanForCircularMotion>("get_plan_circular");
cartesian_planning_srv_client_ = nh.serviceClient<vigir_teleop_planning_msgs::GetMotionPlanForCartesianWaypoints>("get_plan_cartesian");
*/
planner_configuration_.disable_collision_avoidance = false;
planner_configuration_.robot_collision_padding = 0.0f;
planner_configuration_.trajectory_time_factor = 1.0f;
planner_configuration_.octomap_max_height = 2.3f;
planner_configuration_.goal_cube_clearance = 0.105f;
robot_model_loader_.reset(new robot_model_loader::RobotModelLoader());
robot_model_ = robot_model_loader_->getModel();
ros::Duration wait_for_server(50.0);
move_action_client_.reset(new actionlib::SimpleActionClient<vigir_planning_msgs::MoveAction>(nh,
"vigir_move_group",
true));
waitForAction(move_action_client_, wait_for_server, "move");
planner_configuration_sub_ = nh.subscribe("/flor/planning/upper_body/configuration",
10,
&PlanToAction::plannerConfigurationCb,
this);
// General planning ROS API
plan_request_sub_ = nh.subscribe("plan_request", 1, &PlanToAction::planRequestCallback, this);
plan_joint_request_sub_ = nh.subscribe("plan_joint_request", 1, &PlanToAction::planJointRequestCallback, this);
plan_circular_request_sub_ = nh.subscribe("plan_circular_request", 1, &PlanToAction::planCircularRequestCallback, this);
plan_cartesian_request_sub_ = nh.subscribe("plan_cartesian_request", 1, &PlanToAction::planCartesianRequestCallback, this);
plan_status_pub_ = nh.advertise<vigir_ocs_msgs::OCSRobotStatus>("plan_status",1,false);
// Subscriber for plans that sends them of to controller
plan_execute_sub_ = nh.subscribe("execute_trajectory", 1, &PlanToAction::planExecuteCallback, this);
// Subscribers to trajectory messages. For these, the final configuration of the trajectory will be used
// and a new collision free and smooth path there will be planned and sent to controller
general_trajectory_sub_ = nh.subscribe("refine_general_trajectory", 1, &PlanToAction::generalTrajectoryCallback, this);
neck_joint_trajectory_sub_ = nh.subscribe("refine_neck_trajectory", 1, &PlanToAction::refineNeckTrajectoryCallback, this);
torso_joint_trajectory_sub_ = nh.subscribe("refine_torso_trajectory", 1, &PlanToAction::refineTorsoTrajectoryCallback, this);
l_arm_joint_trajectory_sub_ = nh.subscribe("refine_l_arm_trajectory", 1, &PlanToAction::refineLeftArmTrajectoryCallback, this);
r_arm_joint_trajectory_sub_ = nh.subscribe("refine_r_arm_trajectory", 1, &PlanToAction::refineRightArmTrajectoryCallback, this);
// Grasping ROS API
l_grasp_status_pub_ = nh.advertise<vigir_ocs_msgs::OCSRobotStatus>("l_grasp_status",1,false);
r_grasp_status_pub_ = nh.advertise<vigir_ocs_msgs::OCSRobotStatus>("r_grasp_status",1,false);
l_grasp_plan_request_sub_ = nh.subscribe("l_grasp_plan_request", 1, &PlanToAction::lGraspRequestCallback, this);
r_grasp_plan_request_sub_ = nh.subscribe("r_grasp_plan_request", 1, &PlanToAction::rGraspRequestCallback, this);
}
int
main (int argc, char **argv)
{
char **xargv;
int i, j;
int fd;
xargv = alloca (sizeof (char *) * (argc + 32));
if (!read_boolean_pref (CFSTR ("no_auth"), FALSE))
auth_file = XauFileName ();
/* The standard X11 behaviour is for the server to quit when the first
client exits. But it can be useful for debugging (and to mimic our
behaviour in the beta releases) to not do that. */
xinit_kills_server = read_boolean_pref (CFSTR ("xinit_kills_server"), TRUE);
for (i = 1; i < argc; i++)
{
if (argv[i][0] == ':')
server_name = argv[i];
}
if (server_name == NULL)
{
static char name[8];
/* No display number specified, so search for the first unused.
There's a big old race condition here if two servers start at
the same time, but that's fairly unlikely. We could create
lockfiles or something, but that's seems more likely to cause
problems than the race condition itself.. */
for (i = 0; i < MAX_DISPLAYS; i++)
{
if (!display_exists_p (i))
break;
}
if (i == MAX_DISPLAYS)
{
fprintf (stderr, "%s: couldn't allocate a display number", argv[0]);
exit (1);
}
sprintf (name, ":%d", i);
server_name = name;
}
if (auth_file != NULL)
{
/* Create new Xauth keys and add them to the .Xauthority file */
make_auth_keys (server_name);
write_auth_file (TRUE);
}
/* Construct our new argv */
i = j = 0;
xargv[i++] = argv[j++];
if (auth_file != NULL)
{
xargv[i++] = "-auth";
xargv[i++] = auth_file;
}
/* By default, don't listen on tcp sockets if Xauth is disabled. */
if (read_boolean_pref (CFSTR ("nolisten_tcp"), auth_file == NULL))
{
xargv[i++] = "-nolisten";
xargv[i++] = "tcp";
}
while (j < argc)
{
if (argv[j++][0] != ':')
xargv[i++] = argv[j-1];
}
xargv[i++] = (char *) server_name;
xargv[i++] = NULL;
/* Detach from any controlling terminal and connect stdin to /dev/null */
#ifdef TIOCNOTTY
fd = open ("/dev/tty", O_RDONLY);
if (fd != -1)
{
ioctl (fd, TIOCNOTTY, 0);
close (fd);
}
#endif
fd = open ("/dev/null", O_RDWR, 0);
if (fd >= 0)
{
dup2 (fd, 0);
if (fd > 0)
close (fd);
}
if (!start_server (xargv))
return 1;
if (!wait_for_server ())
{
kill (server_pid, SIGTERM);
return 1;
}
if (!start_client ())
{
kill (server_pid, SIGTERM);
return 1;
}
signal (SIGCHLD, sigchld_handler);
signal (SIGTERM, termination_signal_handler);
signal (SIGHUP, termination_signal_handler);
signal (SIGINT, termination_signal_handler);
signal (SIGQUIT, termination_signal_handler);
if (setjmp (exit_continuation) == 0)
{
if (install_ipaddr_source ())
CFRunLoopRun ();
else
while (1) pause ();
}
signal (SIGCHLD, SIG_IGN);
if (client_pid >= 0) kill (client_pid, SIGTERM);
if (server_pid >= 0) kill (server_pid, SIGTERM);
if (auth_file != NULL)
{
/* Remove our Xauth keys */
write_auth_file (FALSE);
}
free_auth_items ();
return 0;
}
int main(int argc, char *argv[], char *dummy[])
{
int c;
int l_mult = 1;
int t_mult = 1;
int req_clients = DEFAULT_CLIENTS;
__u32 server_node;
int client_master_sd;
struct sockaddr_tipc dest_addr;
pid_t child_pid;
unsigned long long num_clients;
unsigned int client_id;
unsigned int server_num;
struct client_cmd cmd = {0,0,0,0,0};
int wait_err;
/* Process command line arguments */
while ((c = getopt(argc, argv, "n:t:l:h")) != -1) {
switch (c) {
case 'l':
l_mult = atoi(optarg);
if (l_mult < 0) {
fprintf(stderr,
"Invalid latency multiplier [%d]\n",
l_mult);
exit(1);
}
break;
case 't':
t_mult = atoi(optarg);
if (t_mult < 0) {
fprintf(stderr,
"Invalid throughput multiplier [%d]\n",
t_mult);
exit(1);
}
break;
case 'n':
req_clients = atoi(optarg);
if (req_clients <= 0) {
fprintf(stderr, "Invalid number of clients "
"[%d]\n", req_clients);
exit(1);
}
break;
default:
usage(argv[0]);
return 1;
}
}
/* Wait for benchmark server to appear */
server_node = wait_for_server(SERVER_NAME, 0, MAX_DELAY);
/* Create socket used to communicate with child clients */
client_master_sd = socket(AF_TIPC, SOCK_RDM, 0);
if (client_master_sd < 0) {
printf("Client master: Can't create main client socket\n");
perror(NULL);
exit(1);
}
dest_addr.family = AF_TIPC;
dest_addr.addrtype = TIPC_ADDR_NAME;
dest_addr.scope = TIPC_NODE_SCOPE;
dest_addr.addr.name.name.type = MCLIENT_NAME;
dest_addr.addr.name.name.instance = 0;
if (bind(client_master_sd, (struct sockaddr *)&dest_addr,
sizeof(dest_addr))) {
printf("Client master: Failed to bind\n");
perror(NULL);
exit(1);
}
printf("****** TIPC benchmark client started ******\n");
num_clients = 0;
dest_addr.addr.name.name.type = CLIENT_NAME;
dest_addr.addr.name.domain = 0;
/* Optionally run latency test */
if (!l_mult)
goto end_latency;
printf("Client master: Starting Latency Benchmark\n");
/* Create first child client */
fflush(stdout);
child_pid = fork();
if (child_pid < 0) {
printf ("Client master: fork failed\n");
perror(NULL);
exit(1);
}
num_clients++;
if (!child_pid) {
close(client_master_sd);
clientmain(num_clients);
/* Note: child client never returns */
}
dprintf("Client master: waiting for confirmation from client 1\n");
wait_err = wait_for_msg(client_master_sd);
if (wait_err) {
printf("Client master: no confirmation from client 1 (err=%u)\n",
wait_err);
exit(1);
}
if (recv(client_master_sd, buf, 4, 0) != 4) {
printf ("Client master: confirmation failure from client 1\n");
perror(NULL);
exit(1);
}
server_num = *(unsigned int *)buf;
dprintf("Client master: client 1 linked to server %i\n", server_num);
/* Run latency test */
cmd.msg_size = 64;
cmd.msg_count = 10240 * l_mult;
cmd.burst_size = cmd.msg_count;
while (cmd.msg_size <= TIPC_MAX_USER_MSG_SIZE) {
struct client_cmd ccmd;
int sz;
struct timeval start_time;
unsigned long long elapsed;
printf("Exchanging %llu messages of size %llu octets (burst size %llu)\n",
cmd.msg_count, cmd.msg_size, cmd.burst_size);
dprintf(" client 1 <--> server %d\n", server_num);
cmd.client_no = 1;
dest_addr.addr.name.name.instance = 1;
gettimeofday(&start_time, 0);
if (sendto(client_master_sd, &cmd, sizeof(cmd), 0,
(struct sockaddr *)&dest_addr, sizeof(dest_addr))
!= sizeof(cmd)) {
printf("Client master: Can't send to client 1\n");
perror(NULL);
exit(1);
}
wait_err = wait_for_msg(client_master_sd);
if (wait_err) {
printf("Client master: No result from client 1 (err=%u)\n",
wait_err);
exit(1);
}
sz = recv(client_master_sd, &ccmd, sizeof(ccmd), 0);
elapsed = elapsedmillis(&start_time);
if (sz != sizeof(ccmd)) {
printf("Client master: invalid result from client 1\n");
perror(NULL);
exit(1);
}
dprintf("Client master:rec cmd msg of size %i [%u:%llu:%llu:%u]\n",
sz,ccmd.cmd,ccmd.msg_size,ccmd.msg_count,ccmd.client_no);
dprintf("Client master: received TASK_FINISHED from client 1\n");
printf("... took %llu ms (round-trip avg/msg: %llu us)\n",
elapsed, (elapsed * 1000)/cmd.msg_count);
cmd.msg_size *= 4;
cmd.msg_count /= 4;
cmd.burst_size /= 4;
}
printf("Client master: Completed Latency Benchmark\n");
end_latency:
/* Optionally run throughput test */
if (!t_mult)
goto end_thruput;
printf("Client master: Starting Throughput Benchmark\n");
/* Create remaining child clients */
while (num_clients < req_clients) {
int sz;
fflush(stdout);
child_pid = fork();
if (child_pid < 0) {
printf ("Client master: fork failed\n");
perror(NULL);
exit(1);
}
num_clients++;
if (!child_pid) {
close(client_master_sd);
clientmain(num_clients);
/* Note: child client never returns */
}
dprintf ("Client master: waiting for confirmation "
"from client %llu\n", num_clients);
wait_err = wait_for_msg(client_master_sd);
if (wait_err) {
printf("Client master: no confirmation from client %llu "
"(err=%u)\n", num_clients, wait_err);
exit(1);
}
sz = recv(client_master_sd, buf, 4, 0);
if (sz != 4) {
printf("Client master: confirmation failure "
"from client_id %llu\n", num_clients);
exit(1);
}
server_num = *(unsigned int*)buf;
dprintf("Client master: client %llu linked to server %i\n",
num_clients, server_num);
}
dprintf("Client master: all clients and servers started\n");
sleep(2); /* let console printfs flush before continuing */
/* Get child clients to run throughput test */
cmd.msg_size = 64;
cmd.msg_count = 10240 * t_mult;
cmd.burst_size = 10240/5;
while (cmd.msg_size < TIPC_MAX_USER_MSG_SIZE) {
struct timeval start_time;
unsigned long long elapsed;
unsigned long long msg_per_sec;
unsigned long long procs;
printf("Exchanging %llu*%llu messages of size %llu octets (burst size %llu)\n",
num_clients, cmd.msg_count, cmd.msg_size, cmd.burst_size);
gettimeofday(&start_time, 0);
for (client_id = 1; client_id <= num_clients; client_id++) {
cmd.client_no = client_id;
dest_addr.addr.name.name.instance = client_id;
if (sendto(client_master_sd, &cmd, sizeof(cmd), 0,
(struct sockaddr *)&dest_addr,
sizeof(dest_addr)) != sizeof(cmd)) {
printf("Client master: can't send to client %u\n",
client_id);
perror(NULL);
exit(1);
}
}
for (client_id = 1; client_id <= num_clients; client_id++) {
struct client_cmd report;
int sz;
wait_err = wait_for_msg(client_master_sd);
if (wait_err) {
printf("Client master: result %u not received "
"(err=%u)\n", client_id, wait_err);
exit(1);
}
sz = recv(client_master_sd, &report, sizeof(report), 0);
if (sz != sizeof(report)) {
printf("Client master: result %u invalid\n",
client_id);
perror(NULL);
exit(1);
}
dprintf("Client master: received TASK_FINISHED "
"from client %u\n", report.client_no);
}
elapsed = elapsedmillis(&start_time);
msg_per_sec = (cmd.msg_count * num_clients * 1000)/elapsed;
procs = 1 + (server_node != own_node(client_master_sd));
printf("... took %llu ms "
"(avg %llu msg/s/dir, %llu bits/s/dir)\n",
elapsed, msg_per_sec/2, msg_per_sec*cmd.msg_size*8/2);
printf(" avg execution time (send+receive) %llu us/msg\n",
(1000000 / (msg_per_sec * 2)) * procs);
cmd.msg_size *= 4;
cmd.msg_count /= 4;
cmd.burst_size /= 4;
}
printf("Client master: Completed Throughput Benchmark\n");
end_thruput:
/* Terminate all client processes */
cmd.cmd = TERMINATE;
for (client_id = 1; client_id <= num_clients; client_id++) {
dest_addr.addr.name.name.instance = client_id;
if (sendto(client_master_sd, &cmd, sizeof(cmd), 0,
(struct sockaddr *)&dest_addr,
sizeof(dest_addr)) <= 0) {
printf("Client master: failed to send TERMINATE message"
" to client %u\n",
client_id);
perror(NULL);
exit(1);
}
}
if (signal(SIGALRM, sig_alarm) == SIG_ERR) {
printf("Client master: Can't catch alarm signals\n");
perror(NULL);
exit(1);
}
alarm(MAX_DELAY);
for (client_id = 1; client_id <= num_clients; client_id++) {
if (wait(NULL) <= 0) {
printf("Client master: error during termination\n");
perror(NULL);
exit(1);
}
}
printf("****** TIPC benchmark client finished ******\n");
shutdown(client_master_sd, SHUT_RDWR);
close(client_master_sd);
exit(0);
}
