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