//--------------------------------------------------------------
void testApp::draw(){
unsigned long start = ofGetElapsedTimeMillis();
bool isButtonPressed = (ard.getDigital(2) == ARD_HIGH);
if (!wasButtonPressed && isButtonPressed) {
cout << "aaa\n";
waitForAction();
beginCapture();
}
if (isCapturing) {
int x = ard.getAnalog(0);
int y = ard.getAnalog(1);
int z = ard.getAnalog(2);
processSample(x, y, z);
}
wasButtonPressed = isButtonPressed;
unsigned long elapsedTime = ofGetElapsedTimeMillis() - start;
if (elapsedTime < samplingInterval) {
delay(samplingInterval - elapsedTime);
}
}
/*
* Constructor
*/
detection::GraspPointDetector::GraspPointDetector(ros::NodeHandle &handle, tf::TransformListener &tf_listener) :
r_arm_group_("right_arm"),
grasp_ranker_(tf_listener),
obj_bounding_box_(new BoundingBox("object_base_frame")),
table_bounding_box_(new BoundingBox("table_base_frame")),
world_obj_(new Cloud),
planar_obj_(new Cloud),
draw_bounding_box_(false),
draw_sampled_grasps_(false),
pub_cluster_(handle.advertise<Cloud>("cluster", 1)),
pub_samples_(handle.advertise<Cloud>("samples", 1)),
tf_listener_(tf_listener),
pick_action_client_(new PickupAction(move_group::PICKUP_ACTION, false)),
nh_(handle),
PLANNER_NAME_(RRTCONNECT_PLANNER())
{
grasp_filter_.reset(new GraspFilter(handle, tf_listener, r_arm_group_, pick_action_client_, PLANNER_NAME_));
waitForAction(pick_action_client_, ros::Duration(5.0), move_group::PICKUP_ACTION);
} // end GraspPointDetector()
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[])
{
command_line_s commandLine;
stream_s *stream = NULL;
connection_list_s *connection;
int browserListener;
fd_set master;
fd_set read_fds;
int fdmax;
int sock;
struct timeval tv;
char buf[BUF_SIZE];
int ret;
int sockcont=0;
port_offset = 0;
signal(SIGINT, sigINThandler);
if (parseCommandLine(argc, argv, &commandLine) < 0)
return -1;
log = open_log(log, "error.log");
log_msg(log, "Created log file\n");
p_log = open_log(p_log, commandLine.logfile);
tv.tv_sec = 60;
tv.tv_usec = 0;
FD_ZERO(&master);
FD_ZERO(&read_fds);
log_msg(log, "Server initiated");
/* set up socket to listen for incoming connections from the browser*/
if (setupBrowserListenerSocket(&browserListener, commandLine.listen_port) < 0)
return EXIT_FAILURE;
/*Add browser listerner to master set of fd's*/
FD_SET(browserListener, &master);
fdmax = browserListener;
log_msg(log, "Browser listener: %d\n", browserListener);
tv.tv_sec = 60;
while (1)
{
//wait for a socket to have data to read
sock = waitForAction(&master,&read_fds, fdmax, tv, sockcont);/*blocking*/
if (sock < 0) continue;//select timeout
if (sock == 0){//read through read_fs , start from beginning
sockcont = 0;
continue;
}
sockcont = sock + 1;
log_msg(log, "Browser requesting a new connection\n");
//Browser is requesting new connection
if (sock == browserListener){ //new connection
if (stream == NULL){
log_msg(log, "Stream is null\n");
}
if (acceptBrowserServerConnectionToStream(browserListener, &master, &fdmax, &stream, &commandLine) < 0)//add connection to stream
//add browser sock to read_fs
return EXIT_FAILURE;
//if no current streams
}
else {
log_msg(log, "Determining if connection is from browser or server...\n");
//Determine if coming from browser or server
connection = getConnectionFromSocket(stream, sock);
if (connection == NULL){
log_msg(log, "Error: NULL connection\n EXIT_FAILURE\n");
return EXIT_FAILURE;
}
if (sock == connection->browser_sock){
log_msg(log, "Found a browser connection\n");
memset(buf, 0, BUF_SIZE);
ret = receive(sock, &master, &fdmax, browserListener, &buf, connection, stream);
int x;
char *get;
char bit[16];
char header[100];
x = strcspn(buf, "\n");
if (x > 99) x = 99;
memcpy(header, buf, x);
header[x] = '\0';
log_msg(log, "Received request from browser:\n%s\n\n", header);
// printf("Received header request from browser:\n%s\n\n", header);
if (ret > 0){
//GET request for .f4m file
//Send GET for ... _nolist.f4m
if ((get = strstr(header, "big_buck_bunny.f4m")) != NULL){
stream->current_throughput = getBitrate(stream->current_throughput, stream->available_bitrates);
unsigned int index = strstr(buf, ".f4m") - buf;
ret = replaceString(buf, ret, "_nolist", index, 0, strlen("_nolist") );
// printf("buf:%s\n", buf);
}
//Get request is for /vod/###SegX-FragY
//Modify for current tput and start new chunk
if ((get = strstr(header, "Seg")) != NULL){
// /vod/###SegX-FragY
char *chunkName;
char chunk[64];
chunkName = strstr(buf, "/vod/");
int intLen = strstr(buf, "Seg") - (chunkName + strlen("/vod/"));
unsigned int index = 0;
int br = getBitrate(stream->current_throughput, stream->available_bitrates); //###
// printf("cur tput: %d, found br: %d\n", stream->current_throughput, br);
snprintf(bit, 16, "%d", br); //convert br into string
ret = replaceString(chunkName + strlen("/vod/"), ret, bit, index, intLen, strlen(bit));
memcpy(chunk, chunkName, strstr(buf, " HTTP") - chunkName);
chunk[strstr(buf, " HTTP") - chunkName] = '\0';
// printf("Chunkname sent to start chunk: %s\n", chunk);
startChunk(connection, chunk);
}
sendResponse(connection->server_sock, buf, ret);
}
}
if (sock == connection->server_sock){
log_msg(log, "Received request from server\n");
memset(buf, 0, BUF_SIZE);
ret = receive(sock, &master, &fdmax, browserListener, &buf, connection, stream);
//Use close to avoid streamlining
char *p, *type, *len, *hlen;
char close[] = "close ";
char contentType[] = "Content-Type: ";
char header[100];
memset(header, '\0', 100);
int x, contentLength;
if (ret <= 0) {
// printf("Continuing!\n");
continue;
}
if (strstr(buf, "Not Modified") != NULL){
exit(4);
}
if (memcmp(buf, "HTTP/1.1", strlen("HTTP/1.1")) == 0){
// printf("Is HTTP\n");
if ((p = strstr(buf, "Connection: ")) != NULL){
p = p + strlen("Connection: ");
memcpy(p, close, strlen(close));
}
// p is a pointer to Content-Type
p = strstr(buf, contentType);
if (p != NULL){
x = strcspn(p, "\n");
if (x > 99) x = 99;
memcpy(header, p, x);
header[x] = '\0';
fflush(stdout);
if ((type = strstr(header, "Content-Type: ")) != NULL){ //found Content-Type
type = type + strlen("Content-Type: ");
}
}
// printf("TYPE:%s\n", type);
if (ret > 0){
// printf("Received %d:\n%s\n",ret, buf);
//If text/xml, set tput to be min
if( strstr(type, "text/xml") != NULL){
stream->current_throughput = getBitrate(stream->current_throughput, stream->available_bitrates);
}
//measure header length until /n/r/n/r and then
//subtract that from ret to see how much data you got and forward
if ( strstr(type, "video/f4f") != NULL){
if ((len = strstr(buf, "Content-Length: ")) != NULL){
len = len + strlen("Content-Length: ");
contentLength = atoi(len);
connection->chunk_throughputs->chunk_size = contentLength;
if ((hlen = strstr(buf, "\r\n\r\n")) != NULL){
hlen = hlen + strlen("\n\r\n\r");
unsigned int h = hlen - buf;
unsigned int bytesRead = ret - h;
// printf("bytesRead: %d\nbytesTotal: %d\n", bytesRead, contentLength);
connection->chunk_throughputs->bytesLeft = contentLength - bytesRead;
}
}
}
sendResponse(connection->browser_sock, buf, ret);
}
} // end IF "HTTP/1.1"
else{ // still reading same request chunk
// printf("Reading video data\nBuf:\n%s\nRet: %d\n", buf, ret);
// printf("Chunk throughputs: %p\n", connection->chunk_throughputs);
if(connection->chunk_throughputs != NULL){
connection->chunk_throughputs->bytesLeft = connection->chunk_throughputs->bytesLeft - ret;
if (connection->chunk_throughputs->bytesLeft == 0){
// printf("***FINISHING CHUNK***\n");
finishChunk(stream, connection, commandLine);
// printf("BYE\n");
}
}
sendResponse(connection->browser_sock, buf, ret);
}
}//end server sock
} // end ELSE
} // end WHILE(1)
close_socket(browserListener);
//endLogger();
return EXIT_SUCCESS;
}
