int main(int argc, char **argv) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. * For programmatic remappings you can use a different version of init() which takes * remappings directly, but for most command-line programs, passing argc and argv is * the easiest way to do it. The third argument to init() is the name of the node. * * You must call one of the versions of ros::init() before using any other * part of the ROS system. */ ros::init(argc, argv, "talker"); /** * NodeHandle is the main access point to communications with the ROS system. * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ ros::NodeHandle n; /** * The advertise() function is how you tell ROS that you want to * publish on a given topic name. This invokes a call to the ROS * master node, which keeps a registry of who is publishing and who * is subscribing. After this advertise() call is made, the master * node will notify anyone who is trying to subscribe to this topic name, * and they will in turn negotiate a peer-to-peer connection with this * node. advertise() returns a Publisher object which allows you to * publish messages on that topic through a call to publish(). Once * all copies of the returned Publisher object are destroyed, the topic * will be automatically unadvertised. * * The second parameter to advertise() is the size of the message queue * used for publishing messages. If messages are published more quickly * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ //ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000); // ros::Publisher chatter_pub = n.advertise<std_msgs::UInt8>("chatter", 1000); ros::Publisher chatter_pub = n.advertise<custom_messages::driveMessage>("chatter", 1000); ros::Subscriber drive_subscriber = n.subscribe("teleop_commands",1000,driveControl_listen); // custom_messages::driveMessage drive_it; drive_it.steering = steering_cmd; drive_it.throttle = throttle_cmd; beginner_tutorials::Num myMsg; myMsg.num = 25; ros::Rate loop_rate(10); // 10 times a second /** * A count of how many messages we have sent. This is used to create * a unique string for each message. */ int count = 0; while (ros::ok()) { /** * This is a message object. You stuff it with data, and then publish it. */ // std_msgs::String msg; std_msgs::UInt8 msg; std::stringstream ss; // ss << "hello world " << count; // msg.data = ss.str(); // *msg.data = 28; msg.data = 28; // ROS_INFO("%s", msg.data.c_str()); ROS_INFO("Steering: %d; Throttle %d", drive_it.steering, drive_it.throttle); /** * The publish() function is how you send messages. The parameter * is the message object. The type of this object must agree with the type * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ chatter_pub.publish(drive_it); ros::spinOnce(); loop_rate.sleep(); ++count; } return 0; }
int main(int argc, char **argv) { // Variables - should be parameters int pub_freq = 10; //Documentation states measurement time is 10times/second std::string port = "/dev/ttyUSB0"; // Initialize Ros ros::init(argc, argv, "stargazer_pose2D"); ros::NodeHandle nh; ros::Publisher stargazer_pub = nh.advertise<geometry_msgs::Pose2D>("stargazer/pose2D", 1); ros::Rate loop_rate(pub_freq); // Initialize StarGazer; StarGazer sg(port); sg.write_parameter( "MarkMode", "Map" ); sg.write_parameter( "RefID", "148" ); ROS_INFO("MarkType: %s", sg.read_parameter("MarkType").c_str()); ROS_INFO("IDNum: %s", sg.read_parameter("IDNum").c_str()); ROS_INFO("RefID: %s", sg.read_parameter("RefID").c_str()); ROS_INFO("HeightFix: %s", sg.read_parameter("HeightFix").c_str()); ROS_INFO("MarkMode: %s", sg.read_parameter("MarkMode").c_str()); ROS_INFO("BaudRate: %s", sg.read_parameter("BaudRate").c_str()); // Velocity calculation variables double velocity[3] = {0, 0, 0}; //{v_x, v_y, omega} double position_i[3] = {-1000, -1000, -1000}; //{x, y, theta} double time_f, time_i = 0; // StarGazer variables geometry_msgs::Pose2D pose; sg.start_calc(); StarGazer::PositionData pd; sg.build_map(5, 148); while(ros::ok()) { pd = sg.get_position().back(); if ( pd.dead ) { std::cout << "dead!\n"; pose.x = -1000; pose.y = -1000; pose.theta = -1000; } else { std::cout << "x: " << pd.x << ", y: " << pd.y << ", z: " << pd.z << ", theta: " << pd.theta << ", id: " << pd.id << "\n"; pose.x = pd.x; pose.y = pd.y; pose.theta = pd.theta; // velocity calculations time_f = ros::Time::now().toSec(); pos_to_vel(pd, velocity, position_i, time_f-time_i); time_i = time_f; std::cout << "v_x: " << velocity[x_ind] << ", v_y: " << velocity[y_ind] << ", omega: " << velocity[angle_ind] << "\n"; } stargazer_pub.publish(pose); } sg.stop_calc(); }
int main(int argc, char **argv) { ros::init(argc, argv, "wall_following_controller"); ros::NodeHandle n; ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000); ros::Subscriber dist_sub = n.subscribe("/ir_publish/sensors", 1000, distanceCallback); ros::Subscriber wallfol_sub = n.subscribe("/wallFollower", 1000, wallFollowerCallback); ros::Rate loop_rate(25); geometry_msgs::Twist msg; srand(time(NULL)); States state = FRONT; int counter = 0; // Booleans that indicate if the sensors on each side detect something close int side=1; while (ros::ok()){ if(wfStateChanged){ wfStateChanged = false; msg.linear.x = 0; msg.angular.z = 0; twist_pub.publish(msg); } if(!wfON){ ros::spinOnce(); loop_rate.sleep(); continue; } bool front = (distance_front_left < front_limit) || (distance_front_right < front_limit); bool right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit); bool left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit); double right_average = (distance_rights_front + distance_rights_back)/2; double left_average = (distance_lefts_front + distance_lefts_back)/2; // Update of the state if(front){ // The front is the most prior if(right_average > left_average){ side = 0; }else{ side = 1; } state = FRONT; std::cerr << "FRONT" << std::endl; } else if (right) { // The right is prefered /*if(state != RIGHT){ direction = rand()%2; std::cerr << direction << std::endl; }*/ state = RIGHT; std::cerr << "RIGHT" << std::endl; } else if (left) { /*if(state != LEFT){ direction = rand()%2; std::cerr << direction << std::endl; }*/ //direction = rand()%1; state = LEFT; std::cerr << "LEFT" << std::endl; } else { state = EMPTY; std::cerr << "EMPTY" << std::endl; } // Action depending on the state switch(state) { case(EMPTY): // Just go straight msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1); break; case(FRONT): msg.linear.x = 0; if(side == 1){ msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1); }else{ msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1); } break; case(RIGHT): msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back); break; case(LEFT): msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back); break; default: break; } // P-controller to generate the angular velocity of the robot, to follow the wall //msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back); //ROS_INFO("Linear velocity :%f", msg.linear.x); //ROS_INFO("Angular velocity :%f", msg.angular.z); if(counter > 10){ twist_pub.publish(msg); }else { counter += 1; } ros::spinOnce(); loop_rate.sleep(); } return 0 ; }
/** * Run this Test */ void PWMBoardTest::spin() { double rate = 5.0d; ros::Rate loop_rate(rate); //herz; double pwmSpeed = 0.0d; //Increment of each speed step double stepSize = 0.25; //Scale factor for setting speed at a lower rate //then the loop double modStepRate = 0.2; //For determining how many times the //loop has "looped" double cntRate = 0.0d; //Positive incrementing first if (maxPWM > 0) { //First, climb to max value bool toMax = true; //Set initial speed setSpeed(0, 0); while (ros::ok()) { //Set speed to pwm speed setSpeed(pwmSpeed, pwmSpeed); ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed); //Check for errors check_EM_STOP(); //Check if it is time for setting new speed if (double_equals(cntRate, rate)) { //Check if max is not reached if (!(double_equals(pwmSpeed, maxPWM)) && (toMax)){ pwmSpeed += stepSize; printf("Stepping up Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM); } //Max is reached else { //Dont go to max value anymore toMax = false; //Check if 0 is not reached if (!double_equals(pwmSpeed, 0.0)) { setSpeed(pwmSpeed,pwmSpeed); pwmSpeed -= stepSize; printf("Stepping down Speed from here: %f %f \n", pwmSpeed, 0.0); } //0 is reached, done else { printf("Done \n"); break; } } //Reset countrate cntRate = 0.0d; std::cout << "cntrate " << cntRate << std::endl; } //Now it not the time for changing speed else { //Increment times loop has looped cntRate += modStepRate; std::cout << "cntrate " << cntRate << std::endl; } loop_rate.sleep(); } } //Negative incrementing(decrementing) first else { //First go to low value bool toMin = true; //Set initial speed setSpeed(0, 0); ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed); while (ros::ok()) { //Set speed to pwm speed setSpeed(pwmSpeed, pwmSpeed); ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed); //Check for errors check_EM_STOP(); //Check if it is time for setting new speed if (double_equals(cntRate, rate)) { //Check if min is not reached if (!(double_equals(pwmSpeed, maxPWM)) && toMin){ pwmSpeed -= stepSize; printf("Stepping down Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM); } //Min is reached else { //Go only up now toMin = false; //Check if 0 is not reached if (!double_equals(pwmSpeed, 0.0)) { setSpeed(pwmSpeed,pwmSpeed); pwmSpeed += stepSize; printf("Stepping up Speed from here: %f %f \n", pwmSpeed, 0.0); } //0 is reached, done else { printf("Done \n"); break; } } //Reset countrate cntRate = 0.0d; std::cout << "cntrate " << cntRate << std::endl; } //Now it not the time for changing speed else { //Increment times loop has looped cntRate += modStepRate; std::cout << "cntrate " << cntRate << std::endl; } loop_rate.sleep(); } } setSpeed(0, 0); ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed); }
int main(int argc, char ** argv) { ros::init(argc, argv, "mocap_pub"); ros::NodeHandle n; int movement = 0; n.getParam("movement", movement); ROS_INFO("Performing movement %d\n", movement); const uint64_t queue_size = 128; // queue size is an arbitrary ros::Publisher mocap_l_base_femur = n.advertise<std_msgs::Float64>("/tl/l_base_link_femur_position_controller/command", queue_size); ros::Publisher mocap_l_femur_tibia = n.advertise<std_msgs::Float64>("/tl/l_femur_tibia_position_controller/command", queue_size); ros::Publisher mocap_l_tibia_feet = n.advertise<std_msgs::Float64>("/tl/l_tibia_feet_position_controller/command", queue_size); ros::Publisher mocap_r_base_femur = n.advertise<std_msgs::Float64>("/tl/r_base_link_femur_position_controller/command", queue_size); ros::Publisher mocap_r_femur_tibia = n.advertise<std_msgs::Float64>("/tl/r_femur_tibia_position_controller/command", queue_size); ros::Publisher mocap_r_tibia_feet = n.advertise<std_msgs::Float64>("/tl/r_tibia_feet_position_controller/command", queue_size); ros::Rate loop_rate(30); // 10 Hz switch(movement) { case 1: { std_msgs::Float64 msg; msg.data = 0; while(ros::ok()) { mocap_l_base_femur.publish(msg); mocap_l_femur_tibia.publish(msg); mocap_l_tibia_feet.publish(msg); mocap_r_base_femur.publish(msg); mocap_r_femur_tibia.publish(msg); mocap_r_tibia_feet.publish(msg); loop_rate.sleep(); ros::spinOnce(); } } break; case 0: { int i = 0; while(ros::ok()) { if (i == 171) i = 0; std_msgs::Float64 msg_l_feet; std_msgs::Float64 msg_l_tibia; std_msgs::Float64 msg_l_femur; std_msgs::Float64 msg_r_feet; std_msgs::Float64 msg_r_tibia; std_msgs::Float64 msg_r_femur; msg_l_feet.data = l_feet[i]; msg_l_tibia.data = l_tibia[i]; msg_l_femur.data = l_femur[i]; msg_r_feet.data = r_feet[i]; msg_r_tibia.data = r_tibia[i]; msg_r_femur.data = r_femur[i]; mocap_l_base_femur.publish(msg_l_feet); mocap_l_femur_tibia.publish(msg_l_tibia); mocap_l_tibia_feet.publish(msg_l_femur); mocap_r_base_femur.publish(msg_r_feet); mocap_r_femur_tibia.publish(msg_r_tibia); mocap_r_tibia_feet.publish(msg_r_femur); i++; ros::spinOnce(); loop_rate.sleep(); } } break; }; ROS_INFO("Movement %d complete\n", movement); return 0; }
int main(int argc, char **argv) { count_loops = 0; ros::init(argc, argv, "convertimagefromHTPApublished"); ros::NodeHandle n; ros::param::set("zoom", 20); //lower_limit = 25; //upper_limit = 45; zoom = 20; HTPAimage = cvCreateImage(cvSize(32*zoom,31*zoom),IPL_DEPTH_8U,3); image_transport::ImageTransport it(n); image_transport::Publisher HTPAimage_pub = it.advertise("HTPAimage", 1); //ros::Subscriber sub = n.subscribe("HTPAoutput", 1000, HTPAoutputCallback); ros::Subscriber sub = n.subscribe("HTPAoutput", 0, HTPAoutputCallback); dynamic_reconfigure::Server<heiman::convertimagefromHTPApublishedConfig> server; dynamic_reconfigure::Server<heiman::convertimagefromHTPApublishedConfig>::CallbackType f; f = boost::bind(&config_callback, _1, _2); server.setCallback(f); ros::Rate loop_rate(10); cv::namedWindow(WINDOW); while (ros::ok()) { count_loops++; if (count_loops == 100) { ROS_WARN("Ten seconds without new HTPA output published"); count_loops = 0; } int zoom_aux; ros::param::get("zoom", zoom_aux); //if (ros::param::has("lower_limit")) //ros::param::get("lower_limit", lower_limit); //if (ros::param::has("upper_limit")) //ros::param::get("upper_limit", upper_limit); if (zoom_aux != zoom) { zoom = zoom_aux; cvReleaseImage(&HTPAimage); HTPAimage = cvCreateImage(cvSize(32*zoom,31*zoom),IPL_DEPTH_8U,3); } // Eli //cv_bridge::CvImage cvi; cv_bridge::CvImagePtr cpt(new cv_bridge::CvImage); ros::Time time = ros::Time::now(); //cvi.header.stamp = time; //cvi.header.frame_id = "image"; //cvi.encoding = "bgr8"; //cvi.image = HTPAimage; cpt->header.stamp = time; cpt->header.frame_id = "image"; cpt->encoding = "bgr8"; cpt->image = HTPAimage; //HTPAimage; //cv::imshow(WINDOW, cpt->image); //cv::waitKey(3); //sensor_msgs::Image msg; //Needs to be ImageConstPtr ??? //cvi.toImageMsg(msg); //cpt->toImageMsg(msg); HTPAimage_pub.publish(cpt->toImageMsg()); //msg); //sensor_msgs::ImagePtr msg = sensor_msgs::cv_bridge::cvToImgMsg(HTPAimage, "bgr8"); //HTPAimage_pub.publish(msg); //------ ros::spinOnce(); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { //Standard ROS startup ros::init(argc, argv, "simulator"); ros::NodeHandle n; //init service to start this node ros::ServiceServer startSimulatorServer = n.advertiseService("start_simulator", startSimulator); ros::Rate r(10); // 10 hz while (!readyToStart) { ros::spinOnce(); r.sleep(); } //setup subscribing to command messages ros::Subscriber sub = n.subscribe("commands", 1000, commandCallback); //setup publishing to telemetry message ros::Publisher telemetryPub = n.advertise<AU_UAV_ROS::TelemetryUpdate>("telemetry", 1000); //setup server services ros::ServiceServer createSimulatedPlaneService = n.advertiseService("create_simulated_plane", createSimulatedPlaneCallback); ros::ServiceServer deleteSimulatedPlaneService = n.advertiseService("delete_simulated_plane", deleteSimulatedPlaneCallback); //setup client services requestPlaneIDClient = n.serviceClient<AU_UAV_ROS::RequestPlaneID>("request_plane_ID"); //TODO:check for validity of 1 Hz //currently updates at 1 Hz, based of Justin Paladino'sestimate of approximately 1 update/sec int loopRate = 1; int loopMultiple = 10; int loopPosition = 0; //we multiply so we can spin more ros::Rate loop_rate(loopMultiple*loopRate); //while the user doesn't kill the process or we get some crazy error while(ros::ok()) { //first check for callbacks ros::spinOnce(); //only run the update once every ten cycles if(loopPosition == 0) { //prep to send some updates AU_UAV_ROS::TelemetryUpdate tUpdate; std::map<int, AU_UAV_ROS::SimulatedPlane>::iterator ii; //iterate through all simulated planes and generate an update for each for(ii = simPlaneMap.begin(); ii != simPlaneMap.end(); ii++) { ii->second.fillTelemetryUpdate(&tUpdate); telemetryPub.publish(tUpdate); } } //sleep until next update cycle loopPosition = (loopPosition+1)%loopMultiple; loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { if(argc!=2) { printf("Usage: break_beam_publisher portname\n"); printf("e.g. break_beam_publisher /dev/ttyACM0\n"); exit(1); } ros::init(argc, argv, "break_beam_publisher"); ros::NodeHandle node; ros::Publisher break_beam_pub = node.advertise<std_msgs::String>("break_beam", 1000); ros::Rate loop_rate(10); std_msgs::String msg; int fd; portname = argv[1]; /* Open the file descriptor in non-blocking mode */ fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); if (fd == -1) { printf("init_serialport %d: Unable to open port %s\n", fd, portname); return -1; } /* Set up the control structure */ struct termios toptions; /* Get currently set options for the tty */ if (tcgetattr(fd, &toptions) < 0) { printf("init_serialport: Couldn't get term attributes\n"); return -1; } /* Set custom options */ /* 9600 baud */ cfsetispeed(&toptions, B9600); cfsetospeed(&toptions, B9600); /* 8 bits, no parity, no stop bits */ toptions.c_cflag &= ~PARENB; toptions.c_cflag &= ~CSTOPB; toptions.c_cflag &= ~CSIZE; toptions.c_cflag |= CS8; /* no hardware flow control */ toptions.c_cflag &= ~CRTSCTS; /* enable receiver, ignore status lines */ toptions.c_cflag |= CREAD | CLOCAL; /* disable input/output flow control, disable restart chars */ toptions.c_iflag &= ~(IXON | IXOFF | IXANY); /* disable canonical input, disable echo, d isable visually erase ch*ars, disable terminal-generated signals */ toptions.c_iflag &= ~(ICANON | ECHO | ECHOE | ISIG); /* disable output processing */ toptions.c_oflag &= ~OPOST; /* wait for 24 characters to come in before read returns */ toptions.c_cc[VMIN] = 0; /* no minimum time to wait before read returns */ toptions.c_cc[VTIME] = 0; /* commit the options */ if( tcsetattr(fd, TCSANOW, &toptions) < 0) { printf("init_serialport: Couldn't set term attributes"); return -1; } /* Wait for the Arduino to reset */ usleep(1000*1000); /* Flush anything already in the serial buffer */ tcflush(fd, TCIFLUSH); /* read up to 128 bytes from the fd */ memset(buf, 0, sizeof(buf)); char b[1]; b[0]='.'; // int n = read(fd, b, 1); int n; int i=0; while (1) { i = 0; do{ n = read(fd, b, 1); while (n <= 0) { n = read(fd, b, 1); } if(b[0]!='\n') buf[i] = b[0]; i++; }while(b[0] != '\n'); if(buf[0]=='U') { std::stringstream ss; ss << "Unbroken"; msg.data = ss.str(); ROS_INFO("publish %s", msg.data.c_str()); break_beam_pub.publish(msg); }else if(buf[0]=='B') { std::stringstream ss; ss << "Broken"; msg.data = ss.str(); ROS_INFO("publish %s", msg.data.c_str()); break_beam_pub.publish(msg); } } return 0; }
int main(int argc, char **argv) { // init ROS and get node-handle ros::init(argc, argv, "feature_constraints_executive"); ros::NodeHandle n; // advertise topics ros::Publisher l_constraint_config_publisher = n.advertise<constraint_msgs::ConstraintConfig>("l_constraint_config", 1, true); ros::Publisher l_constraint_command_publisher = n.advertise<constraint_msgs::ConstraintCommand>("l_constraint_command", 1, true); ros::Publisher r_constraint_config_publisher = n.advertise<constraint_msgs::ConstraintConfig>("r_constraint_config", 1, true); ros::Publisher r_constraint_command_publisher = n.advertise<constraint_msgs::ConstraintCommand>("r_constraint_command", 1, true); ros::Publisher executive_state_publisher = n.advertise<std_msgs::String>("executive_state", 1, true); // subscribe to state topic ros::Subscriber l_constraint_state_subscriber = n.subscribe("l_constraint_state", 1, leftControllerStateCallback); ros::Subscriber r_constraint_state_subscriber = n.subscribe("r_constraint_state", 1, rightControllerStateCallback); // construct messages... // ... features // LEFT constraint_msgs::Feature l_spatula_axis, l_spatula_front, l_spatula_plane; // PANCAKE constraint_msgs::Feature pancake_plane, pancake_right_rim, pancake_right_rim_plane; // RIGHT constraint_msgs::Feature r_spatula_front, r_spatula_plane; // ... constraints // LEFT constraint_msgs::Constraint l_pointing_constraint, l_distance_constraint, l_align_constraint, l_align_front_constraint, l_height_constraint, l_facing_constraint; // RIGHT constraint_msgs::Constraint r_distance_constraint, r_height_constraint, r_align_front_constraint, r_align_side_constraint; // ... entire configurations constraint_msgs::ConstraintConfig l_constraint_config_msg; constraint_msgs::ConstraintConfig r_constraint_config_msg; // ... commands constraint_msgs::ConstraintCommand l_constraint_command_msg; constraint_msgs::ConstraintCommand r_constraint_command_msg; // fill messages... // ... features // LEFT TOOL FEATURES fillLineFeature(l_spatula_axis, "left spatula: main axis", "l_spatula"); l_spatula_axis.direction.z = 0.125; fillLineFeature(l_spatula_front, "left spatula: front axis", "l_spatula"); l_spatula_front.position.z = 0.0475; l_spatula_front.direction.y = 0.125; fillPlaneFeature(l_spatula_plane, "left spatula: plane", "l_spatula"); l_spatula_plane.direction.x = 0.1; l_spatula_plane.contact_direction.z = 0.1; // RIGHT TOOL FEATURES fillLineFeature(r_spatula_front, "right spatula: front axis", "r_spatula"); r_spatula_front.position.z = 0.0475; r_spatula_front.direction.y = 0.125; fillPlaneFeature(r_spatula_plane, "right spatula: plane", "r_spatula"); r_spatula_plane.direction.x = 0.1; r_spatula_plane.contact_direction.z = 0.1; // PANCAKE FEATURES fillPlaneFeature(pancake_plane, "pancake plane", "pancake"); pancake_plane.direction.z = 0.1; pancake_plane.contact_direction.x = 0.1; fillLineFeature(pancake_right_rim, "pancake right rim", "pancake"); pancake_right_rim.position.y = -0.08; pancake_right_rim.direction.x = 0.1; fillPlaneFeature(pancake_right_rim_plane, "pancake right rim plane", "pancake"); pancake_right_rim_plane.direction.z = 0.1; pancake_right_rim_plane.position.y = -0.07; pancake_right_rim_plane.contact_direction.x = 0.1; // ...constraints // LEFT fillPointingAtConstraint(l_pointing_constraint, l_spatula_axis, pancake_plane, "left spatula: at pancake"); fillPerpendicularConstraint(l_align_constraint, l_spatula_axis, pancake_plane, "left spatula: pointing downwards"); fillPerpendicularConstraint(l_align_front_constraint, l_spatula_front, pancake_plane, "left spatula: align front"); fillDistanceConstraint(l_distance_constraint, l_spatula_axis, pancake_plane, "left spatula: distance from pancake"); fillHeightConstraint(l_height_constraint, l_spatula_axis, pancake_plane, "left spatula: keep over"); fillPerpendicularConstraint(l_facing_constraint, l_spatula_plane, pancake_plane, "left spatula: facing pancake"); // RIGHT fillDistanceConstraint(r_distance_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: distance front to pancake rim"); fillHeightConstraint(r_height_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: height front over pancake rim"); fillPerpendicularConstraint(r_align_front_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: align front"); fillPerpendicularConstraint(r_align_side_constraint, r_spatula_plane, pancake_right_rim_plane, "right spatula: align side"); // ... entire configurations // LEFT l_constraint_config_msg.constraints.push_back(l_pointing_constraint); l_constraint_config_msg.constraints.push_back(l_distance_constraint); l_constraint_config_msg.constraints.push_back(l_align_constraint); l_constraint_config_msg.constraints.push_back(l_height_constraint); l_constraint_config_msg.constraints.push_back(l_align_front_constraint); l_constraint_config_msg.constraints.push_back(l_facing_constraint); // RIGHT r_constraint_config_msg.constraints.push_back(r_distance_constraint); r_constraint_config_msg.constraints.push_back(r_height_constraint); r_constraint_config_msg.constraints.push_back(r_align_front_constraint); r_constraint_config_msg.constraints.push_back(r_align_side_constraint); // publish constraint configuration message l_constraint_config_publisher.publish(l_constraint_config_msg); r_constraint_config_publisher.publish(r_constraint_config_msg); // start application which sends different command messages ros::Rate loop_rate(1); ROS_INFO("Starting executive in initial state."); std_msgs::String executive_state_msg; executive_state_msg.data = "start"; executive_state_publisher.publish(executive_state_msg); // another flag which signals that the state machine has finished bool state_machine_finished = false; while(ros::ok() && !state_machine_finished) { // the mean state machine that michael should not see ;) state_machine_finished = updateStateMachineAndWriteCommand(l_constraint_command_msg, r_constraint_command_msg, left_constraints_fulfilled_, right_constraints_fulfilled_); l_constraint_command_publisher.publish(l_constraint_command_msg); r_constraint_command_publisher.publish(r_constraint_command_msg); ros::spinOnce(); loop_rate.sleep(); } executive_state_msg.data = "finish"; executive_state_publisher.publish(executive_state_msg); // bye bye ROS_INFO("Terminating executive."); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, _name); n=(ros::NodeHandle*)new(ros::NodeHandle); init_randomization_seed(); chatter_pub = n->advertise<ws_referee::custom>("player_out", 1); marker_pub = n->advertise<visualization_msgs::Marker>("hamid_marker", 1); br =(tf::TransformBroadcaster*)new (tf::TransformBroadcaster); listener= (tf::TransformListener*) new (tf::TransformListener); init_randomization_seed(); _pos_x=-5; _pos_y=-4; //set teransformation ros::Time t = ros::Time::now(); transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) ); transform.setRotation( tf::Quaternion(0,0,0,1)); br->sendTransform(tf::StampedTransform(transform, t, "world", "tf_"+_name)); tf::Transform tf_tmp; tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) ); tf_tmp.setRotation( tf::Quaternion(0,0,0,1)); br->sendTransform(tf::StampedTransform(tf_tmp, t, "tf_" + _name, "tf_tmp" + _name)); ros::spinOnce(); ros::Duration(0.3).sleep(); //at time now transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name )); tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) ); tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_" + _name, "tf_tmp" + _name )); client= (ros::ServiceClient*) new (ros::ServiceClient); *client = n->serviceClient<ws_referee::MovePlayerTo>("move_player_"+_police_player); ros::ServiceServer service = n->advertiseService("move_player_" + _police_player, serviceCallback); ros::Subscriber sub = n->subscribe("player_in", 1, chatterCallback); ros::Rate loop_rate(2); ROS_INFO("%s: Hamid Node Started", _name.c_str()); int count = 0; ros::MultiThreadedSpinner spinner(4); // Use 4 threads spinner.spin(); // spin() will not return until the node has been shutdown //while (ros::ok()) //{ //ros::spinOnce(); //loop_rate.sleep(); //++count; //} return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "pendulum_controller"); //ros::init(argc, argv, "pendulum"); ros::NodeHandle n("pendulum"); ros::NodeHandle n_state("pendulum"); command_pub = n.advertise<pendulum::command>( "command", 1000 ); state_client = n_state.serviceClient<pendulum::state>( "state" ); //state_client = n.serviceClient<pendulum::state>( "state" ); //state_client = n.serviceClient<pendulum::state>( "state", true ); if( !state_client.isValid( ) ) { ROS_INFO( "state_client not valid" ); state_client.waitForExistence( ); } ros::Rate loop_rate( 25 ); //ros::Rate loop_rate( 10 ); ROS_INFO("Ready to standup pendulum."); command.time = 0.0; command.torque = 0.0; state.request.timestamp = 0.0; pendulum::state s; if( !state_client.isValid( ) ) { ROS_INFO( "state_client no longer valid" ); state_client.waitForExistence( ); } int count = 0; ros::Time calibrate_time, start_time; while( ros::ok( ) ) { calibrate_time = ros::Time::now( ); if( calibrate_time.sec != 0 || calibrate_time.nsec != 0 ) break; ros::spinOnce( ); } start_time = ros::Time::now( ); //ROS_INFO( "start_time: sec[%u] nsec[%u]", start_time.sec, start_time.nsec ); ROS_INFO( "time, position, velocity, torque" ); while( ros::ok( ) ) { //if( count == 1 ) break; if( state_client && state_client.exists( ) && state_client.isValid( ) ) { //ROS_INFO("Calling service"); if( state_client.call( s ) ) { //ROS_INFO("Successful call"); ros::Time sim_time = ros::Time::now( ); //ROS_INFO( "sim_time: sec[%u] nsec[%u]", sim_time.sec, sim_time.nsec ); //ros::Duration current_time = sim_time - start_time; //Real time = (Real) count / 1000.0; Real time = (sim_time - start_time).toSec(); command.torque = control( time, s.response.position, s.response.velocity ); command.time = time; ROS_INFO( "%10.9f, %10.9f, %10.9f, %10.9f", time, s.response.position, s.response.velocity, command.torque ); command_pub.publish( command ); //command.torque = control( state.response.time, state.response.position, state.response.velocity ); //command.time = state.response.time; } else { ROS_INFO("Failed to call"); } } else { ROS_INFO("Persistent client is invalid"); } /* if( !state_client.isValid( ) ) { ROS_INFO( "state_client not valid" ); } if( !state_client.exists( ) ) state_client.waitForExistence( ); if( state_client.call( s ) ) { //if( state_client.call( state ) ) { ROS_INFO( "called state" ); command.torque = control( s.response.time, s.response.position, s.response.velocity ); command.time = s.response.time; //command.torque = control( state.response.time, state.response.position, state.response.velocity ); //command.time = state.response.time; } else { ROS_INFO( "calling state service failed" ); } command_pub.publish( command ); */ ros::spinOnce( ); count++; } ROS_INFO( "Controller shutting down" ); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "amclfakeISL"); ros::NodeHandle n; ros::Publisher amclPosePublisher = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 1000); ros::Subscriber startInfoSubscriber = n.subscribe("communicationISL/neighborInfo",5, startInfoCallback); QString path = QDir::homePath(); path.append("/fuerte_workspace/sandbox/configISL.json"); if(!readConfigFile(path)){ qDebug()<< "Read Config File Failed!!!"; ros::shutdown(); return 0; } ros::Rate loop_rate(loopRate); path = QDir::homePath(); path.append("/fuerte_workspace/sandbox/poses.txt"); if(!readPoses(path)){ qDebug()<< "Read Pose File Failed!!!"; ros::shutdown(); return 0; } /* FILE *fp; fp = fopen("poses.txt","r"); if(fp==NULL) { std::cout <<"Error - could not open poses.txt"; return 0; } else { float x[numOfRobots+1], y[numOfRobots+1]; int i = 0; while (!feof(fp)) { fscanf(fp, "%f %f %f %f %f %f %f %f %f %f", &x[1], &y[1], &x[2], &y[2], &x[3], &y[3], &x[4], &y[4], &x[5], &y[5]); //poses[i][0] = t; poses[i][1] = x[robotID]/100; poses[i][2] = y[robotID]/100; //qDebug()<< poses[i][1]<<" "<<poses[i][2]; i = i + 1; } numOfPoses = i; } std::cout << numOfPoses; */ int timeIndx = 0; while (ros::ok()) { if (startPublishingPose) { if (timeIndx<numOfPoses) { robotPose.pose.pose.position.x = poses[timeIndx][1]; //in meters robotPose.pose.pose.position.y = poses[timeIndx][2]; //in meters robotPose.pose.pose.position.z = 0.1; robotPose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0); amclPosePublisher.publish(robotPose); } /* else { return 0; } */ timeIndx = timeIndx + increment; } ros::spinOnce(); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. * For programmatic remappings you can use a different version of init() which takes * remappings directly, but for most command-line programs, passing argc and argv is * the easiest way to do it. The third argument to init() is the name of the node. * * You must call one of the versions of ros::init() before using any other * part of the ROS system. * * Initializing the node through a call to one of the ros::init() functions. * This provides command line arguments to ROS, and allows you to name your node and specify other options. */ ros::init(argc, argv, "twist_publisher"); /** * NodeHandle is the main access point to communications with the ROS system. * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ ros::NodeHandle n; /** * The advertise() function is how you tell ROS that you want to * publish on a given topic name. This invokes a call to the ROS * master node, which keeps a registry of who is publishing and who * is subscribing. After this advertise() call is made, the master * node will notify anyone who is trying to subscribe to this topic name, * and they will in turn negotiate a peer-to-peer connection with this * node. advertise() returns a Publisher object which allows you to * publish messages on that topic through a call to publish(). Once * all copies of the returned Publisher object are destroyed, the topic * will be automatically unadvertised. * * The second parameter to advertise() is the size of the message queue * used for publishing messages. If messages are published more quickly * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("/riss_bot/cmd_vel", 1); ros::Rate loop_rate(10); while (ros::ok()) { /** * This is a message object. You stuff it with data, and then publish it. */ geometry_msgs::Twist msg; msg.angular.z = 0.02; /** * The publish() function is how you send messages. The parameter * is the message object. The type of this object must agree with the type * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ twist_pub.publish(msg); ros::spinOnce(); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, _name); ros::NodeHandle n; //init the randomizer init_randomization_seed(); chatter_pub = n.advertise<ws_referee::custom>("player_out", 1); marker_pub = n.advertise<visualization_msgs::Marker>("mike_marker", 1); //Allocate the broadcaster and listener br = (tf::TransformBroadcaster*) new (tf::TransformBroadcaster); listener = (tf::TransformListener*) new (tf::TransformListener); _pos_x = -5; _pos_y = 1; //to be read from a param ros::Duration(0.3).sleep(); ros::Time t = ros::Time::now(); //Send the transformation transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(transform, t, "world", "tf_" + _name)); tf::Transform tf_tmp; tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) ); tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(tf_tmp, t, "tf_" + _name, "tf_tmp" + _name)); ros::spinOnce(); ros::Duration(0.3).sleep(); //at time now transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name)); tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) ); tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_" + _name, "tf_tmp" + _name)); ros::ServiceServer service = n.advertiseService("move_player_" + _policed_player, serviceCallback); ros::Subscriber sub = n.subscribe("player_in", 1, chatterCallback); ros::Rate loop_rate(2); ROS_INFO("%s: node started",_name.c_str()); int count = 0; while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); ++count; } return 0; }
int main(int argc, char* argv[]) { ros::init(argc, argv, "jaus"); ros::NodeHandle self; JAUS::Component component; JAUS::Discovery* discoveryService = nullptr; JAUS::Transport* transportService = nullptr; localPoseSensor = new JAUS::LocalPoseSensor(); velocityStateSensor = new JAUS::VelocityStateSensor(); LocalWaypointDriver* localWaypointDriver = new LocalWaypointDriver(self.advertise<sb_msgs::MoveCommand>(MOVE_COMMAND_TOPIC,100),localPoseSensor,nullptr); LocalWaypointListDriver* localWaypointListDriver = new LocalWaypointListDriver(localWaypointDriver); localWaypointDriver->setListDriver(localWaypointListDriver); { JAUS::ReportLocalPose localPose; localPose.SetX(1.0); localPose.SetY(1.0); localPose.SetYaw(0.1); localPoseSensor->SetLocalPose(localPose); } { JAUS::ReportVelocityState state; state.SetVelocityX(2); state.SetYawRate(0.1); velocityStateSensor->SetVelocityState(state); } self.subscribe("robot_state",100,onRobotStateChange); self.subscribe("GPS_COORD",100,onNewWaypoint); component.AddService(localPoseSensor); component.AddService(localWaypointDriver); component.AddService(velocityStateSensor); discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name); discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,"Snowbots"); discoveryService->SetNodeIdentification("Main"); discoveryService->SetComponentIdentification("Baseline"); int comp_id = 5000; JAUS::Address componentID(comp_id,1,1); discoveryService->EnableDebugMessages(true); while(component.Initialize(componentID)==false) { std::cout << "Failed to initialize [" << componentID.ToString() << "]" << std::endl; comp_id++; componentID(comp_id,1,1); } std::cout << "Success!" << std::endl; transportService = (JAUS::Transport*) component.GetService(JAUS::Transport::Name); transportService->LoadSettings("services.xml"); const JAUS::Address comp_address_id = component.GetComponentID(); if(!transportService->IsInitialized()) { transportService->Initialize(comp_address_id); } JAUS::Management* managementService = nullptr; managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name); JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs(); ros::Rate loop_rate(10); while(ros::ok()) { if(managementService->GetStatus() == JAUS::Management::Status::Shutdown) { break; } if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500) { std::cout << "==================" << std::endl; managementService->PrintStatus(); discoveryService->PrintStatus(); transportService->PrintStatus(); std::cout << std::endl; displayStatusTimeMs = JAUS::Time::GetUtcTimeMs(); } ros::spinOnce(); loop_rate.sleep(); } component.Shutdown(); return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "base_controller"); ros::NodeHandle n; ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10); ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, cmd_velCallback); ros::Rate loop_rate(10); last_time = ros::Time::now(); tf::TransformBroadcaster broadcaster; const double degree = M_PI/180; // message declarations geometry_msgs::TransformStamped odom_trans; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_footprint"; while(ros::ok()) { ros::spinOnce(); ros::Time current_time = ros::Time::now(); double dt = (current_time - last_time).toSec(); double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th; geometry_msgs::Quaternion odom_quat; odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th); // update transform odom_trans.header.stamp = current_time; odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th); //filling the odometry nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = "odom"; odom.child_frame_id = "base_footprint"; // position odom.pose.pose.position.x = x; odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; //velocity odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.linear.z = 0.0; odom.twist.twist.angular.x = 0.0; odom.twist.twist.angular.y = 0.0; odom.twist.twist.angular.z = vth; last_time = current_time; // publishing the odometry and the new tf broadcaster.sendTransform(odom_trans); odom_pub.publish(odom); loop_rate.sleep(); } }
int main(int argc, char **argv){ //we need argc and argv for the rosInit function ros::init(argc, argv, "picsubnet"); //inits the driver ros::NodeHandle picsubnetN; //this is what ROS uses to connect to a node /*Advertises our various messages*/ ros::Publisher pubReedSwitch = picsubnetN.advertise<std_msgs::Char>("reed_switch", 100); ros::Publisher pubBTShutdown = picsubnetN.advertise<std_msgs::Char>("bt_shutdown", 100); ros::Publisher pubBatteryVoltage1 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_1", 100); ros::Publisher pubBatteryVoltage2 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_2", 100); ros::Publisher pubAmbient = picsubnetN.advertise<std_msgs::Float32>("ambient_temperature", 100); ros::Publisher pubMotor1 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_1", 100); ros::Publisher pubMotor2 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_2", 100); ros::Publisher pubFitPC = picsubnetN.advertise<std_msgs::Float32>("fitpc_temperature", 100); ros::Publisher pubRouter = picsubnetN.advertise<std_msgs::Float32>("router_temperature", 100); ros::Publisher pubRoboard = picsubnetN.advertise<std_msgs::Float32>("roboard_temperature", 100); ros::Rate loop_rate(15); //how many times a second (i.e. Hz) the code should run if(!open_port()){ ROS_ERROR("FAILED TO CONNECT"); return 0; //we failed to open the port so end } config_port(); ROS_INFO("PIC SUBNET ONLINE"); while (ros::ok()){ //All on/All off led_test++; if (led_test == 1) write_port((unsigned char *)"$1N", 4); else if (led_test == 2) write_port((unsigned char *)"$2N", 4); else if (led_test == 3) write_port((unsigned char *)"$3N", 4); else if (led_test == 4) write_port((unsigned char *)"$4N", 4); else if (led_test == 5) write_port((unsigned char *)"$5N", 4); else if (led_test == 6) write_port((unsigned char *)"$6N", 4); else if (led_test == 7) write_port((unsigned char *)"$7N", 4); else if (led_test == 8) write_port((unsigned char *)"$1F", 4); else if (led_test == 9) write_port((unsigned char *)"$2F", 4); else if (led_test == 10) write_port((unsigned char *)"$3F", 4); else if (led_test == 11) write_port((unsigned char *)"$4F", 4); else if (led_test == 12) write_port((unsigned char *)"$5F", 4); else if (led_test == 13) write_port((unsigned char *)"$6F", 4); else if (led_test == 14) { write_port((unsigned char *)"$7F", 4); led_test = 0; } // Read data from the port if (read_port() == 1) { //if (bt_shutdown.data == (uint8_t)1) ROS_ERROR("BLUETOOTH SHUTDOWN ACTIVATED"); //if (reed_status.data == (uint8_t)-1) ROS_WARN("REED SWITCH - NO RESPONSE"); //There should only ever be 2 batteries connected... int battery_counter = 0; if (battery_voltage1.data != (float)-1.0) { battery1_publish_data.data = battery_voltage1.data; battery_counter++; } if (battery_voltage2.data != (float)-1.0) { if (battery_counter == 0) battery1_publish_data.data = battery_voltage2.data; else battery2_publish_data.data = battery_voltage2.data; battery_counter++; } if (battery_voltage3.data != (float)-1.0) { if (battery_counter == 0) battery1_publish_data.data = battery_voltage3.data; else if (battery_counter == 1) battery2_publish_data.data = battery_voltage3.data; //else //ROS_WARN("More than 2 batteries detected..."); battery_counter++; } if (battery_voltage4.data != (float)-1.0) { if (battery_counter == 0) battery1_publish_data.data = battery_voltage4.data; else if (battery_counter == 1) battery2_publish_data.data = battery_voltage4.data; //else //ROS_WARN("More than 2 batteries detected..."); battery_counter++; } if (battery_counter < 2) ; //ROS_WARN("Only %d Batteries found", battery_counter); else { pubBatteryVoltage1.publish(battery1_publish_data); pubBatteryVoltage2.publish(battery2_publish_data); } //Publish new data pubReedSwitch.publish(reed_status); pubBTShutdown.publish(bt_shutdown); pubAmbient.publish(ambient_temperature); pubMotor1.publish(motor1_temperature); pubMotor2.publish(motor2_temperature); pubFitPC.publish(fitpc_temperature); pubRouter.publish(router_temperature); pubRoboard.publish(roboard_temperature); } /*Have a snooze*/ loop_rate.sleep(); } close(fd); ROS_INFO("Shutting Down"); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "youbot_plane"); ros::NodeHandle n; ros::Subscriber Goal_sub = n.subscribe("GoalPosition",100,&GoalPoseCallback); // subscribe to rostopic GoalPosition ros::Subscriber Current_sub = n.subscribe("odom",100,&CurrentPoseCallback); // subscribe to rostopic odom ros::Publisher Twist_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",100); // publish to /cmd_vel ros::Rate loop_rate(10); control_point c1(0.37,0.1),c2(0.37,0.1),c3(0.37,0.1),c4(0.37,0.1); // use 4 control points // hard code the vertices locations of all 4 obstacles polygon p1(2.0,3.0,2.0,2.0,3.0,2.0,3.0,3.0),p2(4.625,1.375,4.625,0.625,5.375,0.625,5.375,1.375),p3(2.75,5.25,2.75,4.75,3.25,4.75,3.25,5.25),p4(0,3.125,0,2.875,0.125,2.875,0.125,3.125); geometry_msgs::Twist input,control1,control2,control3,control4; // control input for each control point potential pa1,pa2,pa3,pa4,pr1,pr2,pr3,pr4; // attractive/repulsive potential for each control point while (ros::ok() && n.ok()) { ros::spinOnce(); if (STOP == false) { // original names are too long, use shorthand notation here float x,y,theta; x = Current_position.pose.pose.position.x; y = Current_position.pose.pose.position.y; theta = Current_position.pose.pose.orientation.z; // update control point positions in the world frame c1.set_control_point(x+0.3*cos(theta)-0.2*sin(theta),y+0.3*sin(theta)+0.2*cos(theta)); c2.set_control_point(x+0.3*cos(theta)+0.2*sin(theta),y+0.3*sin(theta)-0.2*cos(theta)); c3.set_control_point(x-0.3*cos(theta)+0.2*sin(theta),y-0.3*sin(theta)-0.2*cos(theta)); c4.set_control_point(x-0.3*cos(theta)-0.2*sin(theta),y-0.3*sin(theta)+0.2*cos(theta)); // calculate the attractive potential for each control point pa1 = c1.attractive(Goal_position); pa2 = c2.attractive(Goal_position); pa3 = c3.attractive(Goal_position); pa4 = c4.attractive(Goal_position); // calculate the attractive potential for each control point pr1 = c1.repulsive(p1,p2,p3,p4); pr2 = c2.repulsive(p1,p2,p3,p4); pr3 = c3.repulsive(p1,p2,p3,p4); pr4 = c4.repulsive(p1,p2,p3,p4); // convert the potential to velocity input control1 = c1.controller(Current_position,0.3,0.2,pa1,pr1); control2 = c2.controller(Current_position,0.3,-0.2,pa2,pr2); control3 = c3.controller(Current_position,-0.3,-0.2,pa3,pr3); control4 = c4.controller(Current_position,-0.3,0.2,pa4,pr4); // youbot overall velocity will be the sum of velocity inputs of all four control points input.linear.x = control1.linear.x+ control2.linear.x+control3.linear.x+control4.linear.x; input.linear.y = control1.linear.y+ control2.linear.y+control3.linear.y+control4.linear.y; //input.angular.z = control1.angular.z+control2.angular.z+control3.angular.z+control4.angular.z; //if(input.linear.x>1) input.linear.x = 1; if(input.linear.x<-1) input.linear.x = -1; //if(input.linear.y>1) input.linear.y = 1; if(input.linear.y<-1) input.linear.y = -1; //if(input.angular.z>1) input.angular.z = 1; if(input.angular.z<-1) input.angular.z = -1; Twist_pub.publish(input); // publish the control input to youbot printf("Position x: %f,Position y: %f \n",Current_position.pose.pose.position.x,Current_position.pose.pose.position.y); } else { printf("Waiting...\n"); } loop_rate.sleep(); } ros::spin(); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "joint_states_brazo"); ros::NodeHandle n; ros::Subscriber pose_arm_sub_= n.subscribe("pose_arm", 1, brazo); ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states", 1); ros::Publisher arduino_pub_=n.advertise<std_msgs::String>("arduino_control", 1); ros::Rate loop_rate(100); ros::Time ahora; int cont = 0; ::joint_states_todo.name = std::vector<std::string>(5); ::joint_states_todo.position = std::vector<double>(5); ::joint_states_todo.velocity = std::vector<double>(5); ::joint_states_todo.effort = std::vector<double>(5); while (ros::ok()) { if (::cont == 0) { std_msgs::String brazo; brazo.data = "brazo"; arduino_pub_.publish(brazo); } if (::cont_brazo == 1) { //std::cout<<::joint_states_todo<<std::endl; for (int i = 0; i < 5; i++) { ::joint_states_todo.name[i] = ::name[i]; ::joint_states_todo.position[i] = ::position[i]; ::joint_states_todo.velocity[i] = ::velocity[i]; ::joint_states_todo.effort[i] = ::effort[i]; } ahora = ros::Time::now(); ::joint_states_todo.header.stamp = ahora; joint_states_pub_.publish(::joint_states_todo); ::cont_brazo = 0; } ros::spinOnce(); loop_rate.sleep(); } ros::spin(); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "set_base_velocity_node2"); ros::NodeHandle n; kamui_control::base_velocity bvel; double pre_x, pre_y, pre_yaw; // Leaving直前のロボットの位置、姿勢 double dyna_target; // Rotatingの目標角度 ros::Subscriber urg_sub = n.subscribe("cloud", 50, cloudCallback); //ros::Subscriber odom_sub = n.subscribe("odom", 50, odomCallback); ros::Subscriber robopos_sub = n.subscribe("robot_pos", 50, roboposCallback); ros::Subscriber dyna_sub = n.subscribe("tilt_controller/state", 50, dynaCallback); ros::Subscriber cmdvel_sub = n.subscribe("cmd_vel", 50, cmdVelCallback); ros::Subscriber state_sub = n.subscribe("state", 1000, state_Callback); ros::Subscriber televel_sub = n.subscribe("Velocity_tele", 50, televel_Callback); ros::Publisher pub = n.advertise<kamui_control::base_velocity>("Velocity", 50); ros::ServiceClient client = n.serviceClient<set_state::pubState>("pubState"); set_state::pubState srv; ros::Rate loop_rate(CONTROL_F); bvel.linear = 0; bvel.angular = 0; //ros::param::set("state", 0); // とりあえずNormal while (ros::ok()) { if(state == "Normal") // Normal { bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100); bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100); } else if(state == "Tracking") // Tracking { bvel.linear = 0; bvel.angular = 0; pre_yaw = cur_yaw; dyna_target = cur_dyna_pos; ROS_INFO("Tracking...cur_yaw: %f pre_yaw:%f dyna_target:%f", cur_yaw, pre_yaw, dyna_target);//debug srv.request.state = "GetInfo";//"GetInfo"; client.call(srv); // GetInfoモードへ(robocup2014) } else if(state == "Rotating") // Rotating { //ROS_INFO("Rotating... cur_yaw:%f, pre_yaw:%f", cur_yaw, pre_yaw);//debug if(fabs(cur_yaw - pre_yaw) < fabs(dyna_target)) // 発見してから回転した角度が,ターゲット発見時のカメラの角度を超えると止まる { //回転方向を変える bvel.linear = 0; bvel.angular = (dyna_target > 0)?ROTATIONAL_VEL:-ROTATIONAL_VEL; } else {//ある程度回転したら ROS_INFO("Rotating...cur_yaw: %f pre_yaw:%f dyna_target:%f", cur_yaw, pre_yaw, dyna_target);//debug srv.request.state = "Closing"; client.call(srv); // Closingモードへ } } else if(state == "Closing") // Closing { if(front_dist > CLOSE_TO_VICTIM) { bvel.linear = 2*TRANSLATIONAL_VEL; bvel.angular = 0; //ROS_INFO("Closing");//debug } else { bvel.linear = 0; bvel.angular = 0; srv.request.state = "Scanning"; client.call(srv); // Scanningモードへ } } else if(state == "Scanning") // Scanning { bvel.linear = 0; bvel.angular = 0; //ROS_INFO("Scanning");//debug } else if(state == "GetInfo") // GetInfo { bvel.linear = 0; bvel.angular = 0; pre_x = cur_x; pre_y = cur_y; pre_yaw = cur_yaw; } else if(state == "Leaving") // Leaving { bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100); bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100); ROS_INFO("Leaving");//debug if(sqrt(pow((cur_x-pre_x),2.0)+pow((cur_y-pre_y),2.0)) > DISTCONST || fabs(cur_yaw - pre_yaw) > ANGCONST + fabs(dyna_target)) // 熱源位置から一定距離離れるまでLeavingモード { //cout<<mess.Get_vicnum()+1<<"体目を見つけに行きます"<<endl; if(ros::param::has("/frontier")) { srv.request.state = "Following"; } else { srv.request.state = "Normal"; } client.call(srv); // Normalモードへ } } else if(state == "Stop_temp") // Stop_temp { bvel.linear = 0; bvel.angular = 0; } else if(state == "Waiting") // Waiting { bvel.linear = 0; bvel.angular = 0; } else if(state == "Following") // Following { bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100); bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100); } else if(state == "Moving") // Moving { bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100); bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100); } else if(state == "Teleope") // Teleoperating { bvel.linear = linearVel_tele; bvel.angular = angularVel_tele; } else { bvel.linear = 0; bvel.angular = 0; } pub.publish(bvel); //ROS_INFO("bvel linear: %d, angular: %d", (int)bvel.linear, (int)bvel.angular);//debug ros::spinOnce(); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "uav_commander"); ros::NodeHandle n; ros::NodeHandle n_priv("~"); //////////////////////////// // Load object part types // //////////////////////////// std::vector<std::string> robot_ids; std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation; XmlRpc::XmlRpcValue swarm; n_priv.getParam("swarm", swarm); ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots."); std::map<std::string, XmlRpc::XmlRpcValue>::iterator i; for (i = swarm.begin(); i != swarm.end(); i++) { robot_ids.push_back(i->first); Eigen::Matrix<double,4,1, Eigen::DontAlign> pose; double x=i->second["pose"]["x"]["value"]; double y=i->second["pose"]["y"]["value"]; double z=i->second["pose"]["z"]["value"]; double yaw=i->second["pose"]["yaw"]["value"]; pose << x, y,z, yaw; desired_formation.push_back(pose); } SwarmFormationControl swarm_formation(n,robot_ids,desired_formation); ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10); geometry_msgs::PoseStamped goal_msg; goal_msg.pose.position.x=0.0; goal_msg.pose.position.y=0.0; goal_msg.pose.position.z=1.0; // static tf::TransformBroadcaster brr; // tf::Transform transform; // int increment=1; // tf::Quaternion q; // q.setRPY(0, 0, 0); // transform.setRotation(q); // transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) ); ros::AsyncSpinner spinner(4); spinner.start(); ros::Rate loop_rate(20); while (ros::ok()) { // ++increment; // transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) ); // transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) ); // q.setRPY(0, 0, 0+increment*0.001); // transform.setRotation(q); // brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal")); swarm_formation.updateCentroid(); loop_rate.sleep(); goal_msg.pose.position.x+=0.01; goal_msg.pose.position.z+=0.001; goal_pub.publish(goal_msg); } spinner.stop(); return 0; }
void executeCB(const robot_actionlib::TestGoalConstPtr &goal) { // helper variables ros::Rate loop_rate(25); bool success = true; // push_back the seeds for the fibonacci sequence feedback_.sequence.clear(); feedback_.sequence.push_back(0); feedback_.sequence.push_back(1); // publish info to the console for the user ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]); bool front = 0; bool right = 0; bool left = 0; double right_average = 0; double left_average = 0; while(ros::ok()) { ROS_INFO("Preempted ?: %d ", as_.isPreemptRequested()); ROS_INFO("Active ?: %d ", as_.isActive()); ROS_INFO("GOOALLLLLL: %d ", goal->order); // check that preempt has not been requested by the client if (as_.isPreemptRequested() || !ros::ok()) { ROS_INFO("%s: Preempted", action_name_.c_str()); // set the action state to preempted as_.setPreempted(); success = false; break; } front = (distance_front_left < front_limit) || (distance_front_right < front_limit); right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit); left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit); right_average = (distance_rights_front + distance_rights_back)/2; left_average = (distance_lefts_front + distance_lefts_back)/2; flagReady = true; if (!TestAction::flagReady) { ros::spinOnce(); loop_rate.sleep(); continue; } // Update of the state if(front){ // The front is the most prior if(right_average > left_average){ side = 0; }else{ side = 1; } state = FRONT; std::cerr << "FRONT" << std::endl; } else if (right) { // The right is prefered /*if(state != RIGHT){ direction = rand()%2; std::cerr << direction << std::endl; }*/ state = RIGHT; std::cerr << "RIGHT" << std::endl; } else if (left) { /*if(state != LEFT){ direction = rand()%2; std::cerr << direction << std::endl; }*/ //direction = rand()%1; state = LEFT; std::cerr << "LEFT" << std::endl; } else { state = EMPTY; std::cerr << "EMPTY" << std::endl; } // Action depending on the state switch(state) { case(EMPTY): // Just go straight msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1); break; case(FRONT): msg.linear.x = 0; if(side == 1){ msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1); }else{ msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1); } break; case(RIGHT): msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back); break; case(LEFT): msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05); msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back); break; default: break; } // P-controller to generate the angular velocity of the robot, to follow the wall //msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back); //ROS_INFO("Linear velocity :%f", msg.linear.x); //ROS_INFO("Angular velocity :%f", msg.angular.z); if(counter > 10){ twist_pub.publish(msg); }else { counter += 1; } feedback_.sequence.push_back(feedback_.sequence[1] + feedback_.sequence[0]); // publish the feedback as_.publishFeedback(feedback_); ros::spinOnce(); loop_rate.sleep(); } if(success) { result_.sequence = feedback_.sequence; ROS_INFO("%s: Succeeded", action_name_.c_str()); // set the action state to succeeded as_.setSucceeded(result_); } }
int main(int argc, char **argv) { ros::init(argc, argv, "PersonTracker"); ros::NodeHandle nh; ros::Rate loop_rate(10); ROS_INFO("Initalizing Arm Connection...."); ros::Publisher leftArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n0/position", 1000); ros::Publisher rightArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n1/position", 1000); yourmom = nh.advertise<prlite_kinematics::some_status_thing>("kinect_hack_status", 1000); ros::Duration(2.0).sleep(); prlite_kinematics::SphereCoordinate coord; prlite_kinematics::some_status_thing status; coord.radius = 35.0; coord.phi = 0.0; coord.theta = 0.0; status.lulz = 0; //initialized/reset yourmom.publish(status); leftArmPublisher.publish(coord); rightArmPublisher.publish(coord); ROS_INFO("Initalizing Kinect + NITE...."); XnStatus nRetVal = XN_STATUS_OK; xn::EnumerationErrors errors; nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); if (nRetVal != XN_STATUS_OK) { nRetVal = g_UserGenerator.Create(g_Context); CHECK_RC(nRetVal, "Find user generator"); } XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { printf("Supplied user generator doesn't support skeleton\n"); return 1; } g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks); g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks); if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) { g_bNeedPose = TRUE; if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("Pose required, but not supported\n"); return 1; } g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks); g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose); } g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); nRetVal = g_Context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); ROS_INFO("Ready To Go!\n"); while (ros::ok()) { // Update OpenNI g_Context.WaitAndUpdateAll(); xn::SceneMetaData sceneMD; xn::DepthMetaData depthMD; g_DepthGenerator.GetMetaData(depthMD); g_UserGenerator.GetUserPixels(0, sceneMD); // Print positions XnUserID aUsers[15]; XnUInt16 nUsers = 15; g_UserGenerator.GetUsers(aUsers, nUsers); for (int i = 0; i < nUsers; ++i) { if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) { // Read joint positions for person (No WRISTs) XnSkeletonJointPosition torsoPosition, lShoulderPosition, rShoulderPosition, neckPosition, headPosition, lElbowPosition, rElbowPosition; XnSkeletonJointPosition rWristPosition, lWristPosition, rHipPosition, lHipPosition, lKneePosition, rKneePosition; XnSkeletonJointPosition lFootPosition, rFootPosition; g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_TORSO, torsoPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_NECK, neckPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, headPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, rWristPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HIP, lHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HIP, rHipPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_KNEE, lKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneePosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_FOOT, lFootPosition); g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootPosition); // Read Joint Orientations XnSkeletonJointOrientation torsoOrientation, lShoulderOrientation, rShoulderOrientation, lHipOrientation, rHipOrientation; XnSkeletonJointOrientation lWristOrientation, rWristOrientation, lElbowOrientation, rElbowOrientation; XnSkeletonJointOrientation headOrientation, neckOrientation; XnSkeletonJointOrientation lKneeOrientation, rKneeOrientation, lFootOrientation, rFootOrientation; g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_TORSO, torsoOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_HEAD, headOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_NECK, neckOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HIP, lHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HIP, rHipOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HAND, lWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HAND, rWristOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_KNEE, lKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneeOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_FOOT, lFootOrientation); g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootOrientation); XnFloat* m = torsoOrientation.orientation.elements; KDL::Rotation torsoO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lShoulderOrientation.orientation.elements; KDL::Rotation lShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rShoulderOrientation.orientation.elements; KDL::Rotation rShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lHipOrientation.orientation.elements; KDL::Rotation lHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rHipOrientation.orientation.elements; KDL::Rotation rHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = headOrientation.orientation.elements; KDL::Rotation headO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = neckOrientation.orientation.elements; KDL::Rotation neckO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lElbowOrientation.orientation.elements; KDL::Rotation lElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rElbowOrientation.orientation.elements; KDL::Rotation rElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lWristOrientation.orientation.elements; KDL::Rotation lWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rWristOrientation.orientation.elements; KDL::Rotation rWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lKneeOrientation.orientation.elements; KDL::Rotation lKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rKneeOrientation.orientation.elements; KDL::Rotation rKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = lFootOrientation.orientation.elements; KDL::Rotation lFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); m = rFootOrientation.orientation.elements; KDL::Rotation rFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); double qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0; // Get Points in 3D space XnPoint3D torso = torsoPosition.position; XnPoint3D lShould = lShoulderPosition.position; XnPoint3D rShould = rShoulderPosition.position; XnPoint3D lElbow = lElbowPosition.position; XnPoint3D rElbow = rElbowPosition.position; XnPoint3D lWrist = lWristPosition.position; XnPoint3D rWrist = rWristPosition.position; XnPoint3D neck = neckPosition.position; XnPoint3D head = headPosition.position; XnPoint3D lHip = lHipPosition.position; XnPoint3D rHip = rHipPosition.position; XnPoint3D lKnee = lKneePosition.position; XnPoint3D rKnee = rKneePosition.position; XnPoint3D lFoot = lFootPosition.position; XnPoint3D rFoot = rFootPosition.position; // ---------- ARM CONTROLLER HACK ------------------ // Calculate arm length of user double lenL = calc3DDist( lShould, lElbow ) + calc3DDist( lElbow, lWrist ); double lenR = calc3DDist( rShould, rElbow ) + calc3DDist( rElbow, rWrist ); double distL = calc3DDist(lShould, lWrist); double distR = calc3DDist(rShould, rWrist); // Calculate positions prlite_kinematics::SphereCoordinate lCoord; prlite_kinematics::SphereCoordinate rCoord; lCoord.radius = 35 * (distL / lenL); rCoord.radius = 35 * (distR / lenR); lCoord.theta = -atan2(lShould.X - lWrist.X, lShould.Z - lWrist.Z); rCoord.theta = -atan2(rShould.X - rWrist.X, rShould.Z - rWrist.Z); if(lCoord.theta > 1.0) lCoord.theta = 1.0; if(lCoord.theta < -1.0) lCoord.theta = -1.0; if(rCoord.theta > 1.0) rCoord.theta = 1.0; if(rCoord.theta < -1.0) rCoord.theta = -1.0; lCoord.phi = -atan2(lShould.Y - lWrist.Y, lShould.X - lWrist.X); rCoord.phi = -atan2(rShould.Y - rWrist.Y, rShould.X - rWrist.X); if(lCoord.phi > 1.25) lCoord.phi = 1.25; if(lCoord.phi < -0.33) lCoord.phi = -0.33; if(rCoord.phi > 1.2) rCoord.phi = 1.25; if(rCoord.phi < -0.33) rCoord.phi = -0.33; ROS_INFO("User %d: Left (%lf,%lf,%lf), Right (%lf,%lf,%lf)", i, lCoord.radius, lCoord.theta, lCoord.phi, rCoord.radius, rCoord.theta, rCoord.phi); // Publish to arms leftArmPublisher.publish(rCoord); rightArmPublisher.publish(lCoord); // ---------- END HACK ----------------- // Publish Transform static tf::TransformBroadcaster br; tf::Transform transform; std::stringstream body_name, neck_name, lshould_name, rshould_name, head_name, relbow_name, lelbow_name, lwrist_name, rwrist_name; std::stringstream lhip_name, rhip_name, lknee_name, rknee_name, lfoot_name, rfoot_name; body_name << "Torso (" << i << ")" << std::ends; neck_name << "Neck (" << i << ")" << std::ends; head_name << "Head (" << i << ")" << std::ends; lshould_name << "Shoulder L (" << i << ")" << std::ends; rshould_name << "Shoulder R (" << i << ")" << std::ends; lelbow_name << "Elbow L (" << i << ")" << std::ends; relbow_name << "Elbow R (" << i << ")" << std::ends; lwrist_name << "Wrist L (" << i << ")" << std::ends; rwrist_name << "Wrist R (" << i << ")" << std::ends; lhip_name << "Hip L (" << i << ")" << std::ends; rhip_name << "Hip R (" << i << ")" << std::ends; lknee_name << "Knee L (" << i << ")" << std::ends; rknee_name << "Knee R (" << i << ")" << std::ends; lfoot_name << "Foot L (" << i << ")" << std::ends; rfoot_name << "Foot R (" << i << ")" << std::ends; // Publish Torso torsoO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(torso.X / ADJUST_VALUE, torso.Y / ADJUST_VALUE, torso.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", body_name.str().c_str())); // Publish Left Shoulder lShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lShould.X / ADJUST_VALUE, lShould.Y / ADJUST_VALUE, lShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lshould_name.str().c_str())); // Publish Right Shoulder rShouldO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rShould.X / ADJUST_VALUE, rShould.Y / ADJUST_VALUE, rShould.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rshould_name.str().c_str())); // Publish Left Elbow lElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lElbow.X / ADJUST_VALUE, lElbow.Y / ADJUST_VALUE, lElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lelbow_name.str().c_str())); // Publish Right Elbow rElbowO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rElbow.X / ADJUST_VALUE, rElbow.Y / ADJUST_VALUE, rElbow.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", relbow_name.str().c_str())); // Publish Left Wrist lWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lWrist.X / ADJUST_VALUE, lWrist.Y / ADJUST_VALUE, lWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lwrist_name.str().c_str())); // Publish Right Wrist rWristO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rWrist.X / ADJUST_VALUE, rWrist.Y / ADJUST_VALUE, rWrist.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rwrist_name.str().c_str())); // Publish Neck neckO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(neck.X / ADJUST_VALUE, neck.Y / ADJUST_VALUE, neck.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", neck_name.str().c_str())); // Publish Head headO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(head.X / ADJUST_VALUE, head.Y / ADJUST_VALUE, head.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", head_name.str().c_str())); // Publish Left Hip lHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lHip.X / ADJUST_VALUE, lHip.Y / ADJUST_VALUE, lHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lhip_name.str().c_str())); // Publish Right Hip rHipO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rHip.X / ADJUST_VALUE, rHip.Y / ADJUST_VALUE, rHip.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rhip_name.str().c_str())); // Publish Left Knee lKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lKnee.X / ADJUST_VALUE, lKnee.Y / ADJUST_VALUE, lKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lknee_name.str().c_str())); // Publish Right Knee rKneeO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rKnee.X / ADJUST_VALUE, rKnee.Y / ADJUST_VALUE, rKnee.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rknee_name.str().c_str())); // Publish Left Foot lFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(lFoot.X / ADJUST_VALUE, lFoot.Y / ADJUST_VALUE, lFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lfoot_name.str().c_str())); // Publish Right Foot rFootO.GetQuaternion(qx, qy, qz, qw); transform.setOrigin(tf::Vector3(rFoot.X / ADJUST_VALUE, rFoot.Y / ADJUST_VALUE, rFoot.Z / ADJUST_VALUE)); transform.setRotation(tf::Quaternion(qx, qy, qz, qw)); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rfoot_name.str().c_str())); } } ros::spinOnce(); //loop_rate.sleep(); } g_Context.Shutdown(); return 0; }
/** * This tutorial demonstrates simple sending of messages over the ROS system. */ int main(int argc, char **argv) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. For programmatic * remappings you can use a different version of init() which takes remappings * directly, but for most command-line programs, passing argc and argv is the easiest * way to do it. The third argument to init() is the name of the node. * * You must call one of the versions of ros::init() before using any other * part of the ROS system. */ // %Tag(INIT)% ros::init(argc, argv, "circleScanner"); // %EndTag(INIT)% /** * NodeHandle is the main access point to communications with the ROS system. * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ // %Tag(NODEHANDLE)% ros::NodeHandle n; // %EndTag(NODEHANDLE)% /** * The advertise() function is how you tell ROS that you want to * publish on a given topic name. This invokes a call to the ROS * master node, which keeps a registry of who is publishing and who * is subscribing. After this advertise() call is made, the master * node will notify anyone who is trying to subscribe to this topic name, * and they will in turn negotiate a peer-to-peer connection with this * node. advertise() returns a Publisher object which allows you to * publish messages on that topic through a call to publish(). Once * all copies of the returned Publisher object are destroyed, the topic * will be automatically unadvertised. * * The second parameter to advertise() is the size of the message queue * used for publishing messages. If messages are published more quickly * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ // %Tag(PUBLISHER)% ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000); // %EndTag(PUBLISHER)% // %Tag(LOOP_RATE)% ros::Rate loop_rate(10); // %EndTag(LOOP_RATE)% /** * A count of how many messages we have sent. This is used to create * a unique string for each message. */ // %Tag(ROS_OK)% int count = 0; while (ros::ok()) { // %EndTag(ROS_OK)% /** * This is a message object. You stuff it with data, and then publish it. */ // %Tag(FILL_MESSAGE)% std_msgs::String msg; std::stringstream ss; ss << "hello world " << count; msg.data = ss.str(); // %EndTag(FILL_MESSAGE)% // %Tag(ROSCONSOLE)% ROS_INFO("%s", msg.data.c_str()); // %EndTag(ROSCONSOLE)% /** * The publish() function is how you send messages. The parameter * is the message object. The type of this object must agree with the type * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ // %Tag(PUBLISH)% chatter_pub.publish(msg); // %EndTag(PUBLISH)% // %Tag(SPINONCE)% ros::spinOnce(); // %EndTag(SPINONCE)% // %Tag(RATE_SLEEP)% loop_rate.sleep(); // %EndTag(RATE_SLEEP)% ++count; } return 0; }
/** * This tutorial demonstrates simple sending of messages over the ROS system. */ int main(int argc, char **argv) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. * For programmatic remappings you can use a different version of init() which takes * remappings directly, but for most command-line programs, passing argc and argv is * the easiest way to do it. The third argument to init() is the name of the node. * * You must call one of the versions of ros::init() before using any other * part of the ROS system. */ ros::init(argc, argv, "talker"); /** * NodeHandle is the main access point to communications with the ROS system. * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ ros::NodeHandle n; /** * The advertise() function is how you tell ROS that you want to * publish on a given topic name. This invokes a call to the ROS * master node, which keeps a registry of who is publishing and who * is subscribing. After this advertise() call is made, the master * node will notify anyone who is trying to subscribe to this topic name, * and they will in turn negotiate a peer-to-peer connection with this * node. advertise() returns a Publisher object which allows you to * publish messages on that topic through a call to publish(). Once * all copies of the returned Publisher object are destroyed, the topic * will be automatically unadvertised. * * The second parameter to advertise() is the size of the message queue * used for publishing messages. If messages are published more quickly * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ ros::Publisher chatter_pub = n.advertise<ackermann_msgs::AckermannDrive>("chatter", 1000); ros::Rate loop_rate(0.5); /** * A count of how many messages we have sent. This is used to create * a unique string for each message. */ int count = 0; int steering_angle = 0; int speed = 0; while (ros::ok()) { /** * This is a message object. You stuff it with data, and then publish it. */ ackermann_msgs::AckermannDrive msg; msg.speed = 10; if (abs(steering_angle) == 1) { steering_angle = 0; speed = 0; } else if (count % 3 == 1) { steering_angle = -1; speed = -1; } else { steering_angle = 1; speed = 1; } msg.steering_angle = steering_angle; msg.speed = steering_angle; ROS_INFO("Sent: [%f] speed, [%f] steering", msg.speed, msg.steering_angle); /** * The publish() function is how you send messages. The parameter * is the message object. The type of this object must agree with the type * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ chatter_pub.publish(msg); ros::spinOnce(); loop_rate.sleep(); ++count; } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "sonar_node_2"); ros::NodeHandle n; ros::Publisher chatter_pub = n.advertise<sonar::Sonar>("sonar_data", 1000); ros::Rate loop_rate(25); float distance1 = 0; float distance2 = 0; float distance3 = 0; // float distance4 = 0; // float distance5 = 0; // float distance6 = 0; wiringPiSetup(); pinMode(21, OUTPUT); pinMode(22, OUTPUT); pinMode(23, OUTPUT); // pinMode(24, OUTPUT); // pinMode(25, OUTPUT); // pinMode(26, OUTPUT); digitalWrite(21, HIGH); digitalWrite(22, LOW); digitalWrite(23, LOW); // digitalWrite(24, LOW); // digitalWrite(25, LOW); // digitalWrite(26, LOW); wiringPiISR(0,INT_EDGE_BOTH,&Interrupt1); wiringPiISR(1,INT_EDGE_BOTH,&Interrupt2); wiringPiISR(2,INT_EDGE_BOTH,&Interrupt3); // wiringPiISR(3,INT_EDGE_BOTH,&Interrupt4); // wiringPiISR(4,INT_EDGE_BOTH,&Interrupt5); // wiringPiISR(5,INT_EDGE_BOTH,&Interrupt6); while (ros::ok()) { if(sonar1_stop_time > sonar1_start_time) { sonar1_time = sonar1_stop_time - sonar1_start_time; } if(sonar2_stop_time > sonar2_start_time) { sonar2_time = sonar2_stop_time - sonar2_start_time; } if(sonar3_stop_time > sonar3_start_time) { sonar3_time = sonar3_stop_time - sonar3_start_time; } /* if(sonar4_stop_time > sonar4_start_time) { sonar4_time = sonar4_stop_time - sonar4_start_time; } if(sonar5_stop_time > sonar5_start_time) { sonar5_time = sonar5_stop_time - sonar5_start_time; } if(sonar6_stop_time > sonar6_start_time) { sonar6_time = sonar6_stop_time - sonar6_start_time; } */ distance1 = sonar1_time/58; distance2 = sonar2_time/58; distance3 = sonar3_time/58; // distance4 = sonar4_time/58; // distance5 = sonar5_time/58; // distance6 = sonar6_time/58; sonar_pprev = sonar_prev; sonar_prev = sonar_raw; sonar_raw.sonar_1 = distance1; if(distance1>600) { sonar_raw.sonar_1 = 600; } if(((distance1-sonar_prev.sonar_1)>300)&&(sonar_prev.sonar_1>0)&&(sonar_pprev.sonar_1>0)) { sonar_raw.sonar_1 = sonar_prev.sonar_1; } sonar_raw.sonar_2 = distance2; if(distance2>600) { sonar_raw.sonar_2 = 600; } if(((distance2-sonar_prev.sonar_2)>300)&&(sonar_prev.sonar_2>0)&&(sonar_pprev.sonar_2>0)) { sonar_raw.sonar_2 = sonar_prev.sonar_2; } sonar_raw.sonar_3 = distance3; if(distance3>600) { sonar_raw.sonar_3 = 600; } if(((distance3-sonar_prev.sonar_3)>300)&&(sonar_prev.sonar_3>0)&&(sonar_pprev.sonar_3>0)) { sonar_raw.sonar_3 = sonar_prev.sonar_3; } /* sonar_raw.sonar_4 = distance4; if(distance4>600) { sonar_raw.sonar_4 = 600; } if(((distance4-sonar_prev.sonar_4)>300)&&(sonar_prev.sonar_4>0)&&(sonar_pprev.sonar_4>0)) { sonar_raw.sonar_4 = sonar_prev.sonar_4; } sonar_raw.sonar_5 = distance5; if(distance5>600) { sonar_raw.sonar_5 = 600; } if(((distance5-sonar_prev.sonar_5)>300)&&(sonar_prev.sonar_5>0)&&(sonar_pprev.sonar_5>0)) { sonar_raw.sonar_5 = sonar_prev.sonar_5; } sonar_raw.sonar_6 = distance6; if(distance6>600) { sonar_raw.sonar_6 = 600; } if(((distance6-sonar_prev.sonar_6)>300)&&(sonar_prev.sonar_6>0)&&(sonar_pprev.sonar_6>0)) { sonar_raw.sonar_6 = sonar_prev.sonar_6; } */ sonar_filtered.sonar_1 = (sonar_raw.sonar_1 + sonar_prev.sonar_1 + sonar_pprev.sonar_1)/3; sonar_filtered.sonar_2 = (sonar_raw.sonar_2 + sonar_prev.sonar_2 + sonar_pprev.sonar_2)/3; sonar_filtered.sonar_3 = (sonar_raw.sonar_3 + sonar_prev.sonar_3 + sonar_pprev.sonar_3)/3; // sonar_filtered.sonar_4 = (sonar_raw.sonar_4 + sonar_prev.sonar_4 + sonar_pprev.sonar_4)/3; // sonar_filtered.sonar_5 = (sonar_raw.sonar_5 + sonar_prev.sonar_5 + sonar_pprev.sonar_5)/3; // sonar_filtered.sonar_6 = (sonar_raw.sonar_6 + sonar_prev.sonar_6 + sonar_pprev.sonar_6)/3; chatter_pub.publish(sonar_filtered); ros:: spinOnce(); loop_rate.sleep(); } return 0; }
int main(int argc, char **argv){ //we need argc and argv for the rosInit function ros::init(argc, argv, "compass"); //inits the driver ros::NodeHandle n; //this is what ROS uses to connect to a node /*Advertises our various messages*/ ros::Publisher compassHeadingMsg = n.advertise<std_msgs::Float32>("compassHeading", 100); ros::Publisher compassPitchMsg = n.advertise<std_msgs::Float32>("compassPitch", 100); ros::Publisher compassRollMsg = n.advertise<std_msgs::Float32>("compassRoll", 100); /*Sets up the message structures*/ std_msgs::Float32 compassHeading; std_msgs::Float32 compassPitch; std_msgs::Float32 compassRoll; ros::Rate loop_rate(10); //how many times a second (i.e. Hz) the code should run if(!open_port()){ return 0; //we failed to open the port so end } config_port(); ROS_INFO("Compass Driver Online"); while (ros::ok()){ if(write_port()){ //if we send correctly if(read_port() != 0){ //if we read correctly parseBuffer(); //parse the buffer //printf("H: %f P: %f R: %f\n",heading,pitch,roll); /* Below here sets up the messages ready for transmission*/ compassHeading.data = heading; compassPitch.data = pitch; compassRoll.data = roll; ROS_DEBUG("H: %.3f P: %.3f R: %.3f",heading,pitch,roll); } else{ ROS_ERROR("Read no data"); } } else{ ROS_ERROR("Failed to write"); } /*Below here we publish our readings*/ compassHeadingMsg.publish(compassHeading); compassRollMsg.publish(compassRoll); compassPitchMsg.publish(compassPitch); /*Have a snooze*/ loop_rate.sleep(); } ros::spin(); close(fd); ROS_INFO("Shutting Down"); printf("Shutting Down\n"); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "short_moves_pub"); control move_control; ros::NodeHandle n; ros::Publisher short_moves_pub = n.advertise<std_msgs::Bool>("/interrupt", 100); //publisher to new topic interrupt ros::Rate loop_rate(1000); // (1 kHz) std_msgs::Bool msg; unsigned int count = 0; while (ros::ok()) //this returns false if e.g. CTRL-C is hit { /*if (count == 500) //first accelerate { msg.data = true; ROS_INFO("stop wii"); short_moves_pub.publish(msg); //publish to interrupt topic the value false, which then indicates to wii_comunication to stop it's publishing to servo (due to collision reasons) move_control.control_servo.x = 1550; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 600) { move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 700) { move_control.control_servo.x = 1555; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 1500) //then break { move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 2000) //after that move backwards... { move_control.control_servo.x = 1300; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 2100) { move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 2200) { move_control.control_servo.x = 1450; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 2700) //and break { msg.data = false; ROS_INFO("free wii"); //make wii_comunication know that it now can send to servo topic again by sending false short_moves_pub.publish(msg); move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected break; } */ if (count == 500) //move backwards { msg.data = true; ROS_INFO("stop wii"); short_moves_pub.publish(msg); //publish to interrupt topic the value false, which then indicates to wii_comunication to stop it's publishing to servo (due to collision reasons) move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 600) { move_control.control_servo.x = 1450; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 700) { move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 800) { move_control.control_servo.x = 1450; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected } else if (count == 1800) //and break { msg.data = false; ROS_INFO("free wii"); //make wii_comunication know that it now can send to servo topic again by sending false short_moves_pub.publish(msg); move_control.control_servo.x = 1500; move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected //ros::spinOnce(); //provide callbacks //loop_rate.sleep(); } else if(count == 2000) { break; } ros::spinOnce(); //provide callbacks count++; loop_rate.sleep(); } return 0; }
int main(int argc, char **argv) { int ros_argc; char **ros_argv; char node_name[NODE_NAME_LEN] = NODE_NAME_DEFAULT; int retval; int option; double dval; nist_kitting::ws_stat ws_stat_buf; ws_stat_buf.stat.period = PERIOD_DEFAULT; opterr = 0; while (true) { option = getopt(argc, argv, ":n:p:hd"); if (option == -1) break; switch (option) { case 'n': // first check for valid name if (optarg[0] == '-') { fprintf(stderr, "invalid node name: %s\n", optarg); return 1; } strncpy(node_name, optarg, NODE_NAME_LEN-1); node_name[NODE_NAME_LEN-1] - 0; break; case 'p': dval = atof(optarg); if (dval < FLT_EPSILON) { fprintf(stderr, "bad value for period: %s\n", optarg); return 1; } ws_stat_buf.stat.period = dval; break; case 'h': print_help(); break; case 'd': debug = 1; break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } } // pass everything after a '--' separator to ROS ros_argc = argc - optind; ros_argv = &argv[optind]; ros::init(ros_argc, ros_argv, node_name); ros::NodeHandle nh; ros::Subscriber ws_cmd_sub; ros::Subscriber task_stat_sub; ros::Publisher ws_stat_pub; ros::Publisher task_cmd_pub; ros::Rate loop_rate(1.0 / ws_stat_buf.stat.period); ws_cmd_sub = nh.subscribe(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN, ws_cmd_callback); task_stat_sub = nh.subscribe(KITTING_TASK_STAT_TOPIC, TOPIC_QUEUE_LEN, task_stat_callback); ws_stat_pub = nh.advertise<nist_kitting::ws_stat>(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN); task_cmd_pub = nh.advertise<nist_kitting::task_cmd>(KITTING_TASK_CMD_TOPIC, TOPIC_QUEUE_LEN); // stuff a NOP command ws_stat_buf.stat.type = ws_cmd_buf.cmd.type = KITTING_NOP; ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number = 0; ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND; ws_stat_buf.stat.status = RCS_STATUS_EXEC; ws_stat_buf.stat.heartbeat = 0; signal(SIGINT, quit); double start, end, last_start = ulapi_time() - ws_stat_buf.stat.period; while (true) { ros::spinOnce(); start = ulapi_time(); ws_stat_buf.stat.cycle = start - last_start; last_start = start; if (ws_stat_buf.stat.serial_number != ws_cmd_buf.cmd.serial_number) { ws_stat_buf.stat.type = ws_cmd_buf.cmd.type; ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number; ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND; ws_stat_buf.stat.status = RCS_STATUS_EXEC; } switch (ws_cmd_buf.cmd.type) { case KITTING_NOP: do_cmd_kitting_nop(ws_stat_buf); break; case KITTING_INIT: do_cmd_kitting_init(ws_stat_buf, task_cmd_pub, task_stat_buf); break; case KITTING_HALT: do_cmd_halt(ws_stat_buf); break; case KITTING_WS_ASSEMBLE_KIT: do_cmd_kitting_ws_assemble_kit(ws_cmd_buf.assemble_kit, ws_stat_buf); break; default: // unrecognized command -- FIXME break; } ws_stat_buf.stat.heartbeat++; end = ulapi_time(); ws_stat_buf.stat.duration = ulapi_time() - start; ws_stat_pub.publish(ws_stat_buf); loop_rate.sleep(); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "file_publisher"); ros::NodeHandle nh; // Declare variables that can be modified by launch file or command line. std::string file; int frequency; // Setting parameters with default values nh.setParam("file", std::string("/home/young/文档/test_pics/ball/level4/blurry/frame"));// nh.setParam("frequency", int(1)); // Getting the values of parameters if set from launch file or command line nh.getParam("file", file); nh.getParam("frequency", frequency); image_transport::ImageTransport it(nh); image_transport::Publisher pub = it.advertise("image", 1); ros::Rate loop_rate(frequency); ROS_INFO("Ready to publish loaded image from directory:[%s] of frequency:[%d]", file.c_str(), frequency); int i; i = START_NUM; // 根据读入的文件名进行修改/home/young/文档/test_pics/ball/level1/clear/frame g_format.parse("%s%04d.%s"); while (nh.ok()) { if (i == END_NUM) i = START_NUM;// 根据读入的文件名进行修改/home/young/文档/test_pics/ball/level1/clear/frame std::string filename = (g_format %file % i % "jpg").str(); /*std::stringstream ss; ss << file << i << ".bmp"; ss >> filename;*/ //sprintf(filename, "/home/chy/pictures/%d.bmp", i); ROS_INFO("Ready to publish loaded image from file:[%s]", filename.c_str()); // Loading the image, converting it to cv_bridge::CvImage type and to sensor_msgs::ImagePtr using .toImageMsg() // cv::WImageBuffer3_b image( cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR) ); // cv::Mat imageMat(image.Ipl()); cv::Mat imageMat = cv::imread(filename.c_str(), 1); if(!imageMat.data) { continue; } cv_bridge::CvImage out_msg; out_msg.encoding = "bgr8"; out_msg.encoding = sensor_msgs::image_encodings::BGR8; out_msg.image = imageMat; // out_msg.header.seq = i; // out_msg.header.frame_id = i; out_msg.header.stamp = ros::Time::now(); // ci->header.seq = i; // ci->header.frame_id = i; // ci->header.stamp = out_msg.header.stamp; sensor_msgs::ImagePtr msg = out_msg.toImageMsg(); pub.publish(msg); ros::spinOnce(); loop_rate.sleep(); //image.ReleaseImage(); //imageMat.release(); i++; } }