void MediaPlayerPrivateAVFoundation::timeChanged(double time) { LOG(Media, "MediaPlayerPrivateAVFoundation::timeChanged(%p) - time = %f", this, time); if (m_seekTo == invalidTime) return; // AVFoundation may call our observer more than once during a seek, and we can't currently tell // if we will be able to seek to an exact time, so assume that we are done seeking if we are // "close enough" to the seek time. const double smallSeekDelta = 1.0 / 100; float currentRate = rate(); if ((currentRate > 0 && time >= m_seekTo) || (currentRate < 0 && time <= m_seekTo) || (abs(m_seekTo - time) <= smallSeekDelta)) { m_seekTo = invalidTime; updateStates(); m_player->timeChanged(); } }
int main(int argc, char** argv){ ros::init(argc, argv, "pose_to_tf"); ros::NodeHandle node("~"); node.param("frame_id", frame_id, std::string("frame_id_not_specified")); tf::TransformBroadcaster br; started = false; ros::NodeHandle node_top; ros::Subscriber sub = node_top.subscribe("pose", 10, &poseCallback); ros::Rate rate(50.0); while (ros::ok()){ if (started) br.sendTransform(transform); ros::spinOnce(); rate.sleep(); } return 0; }
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_broadcaster"); ros::NodeHandle node; tf::TransformBroadcaster br; tf::Transform transform; ros::Rate rate(10.0); while (node.ok()){ transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1")); rate.sleep(); } return 0; };
int main(int argc, char** argv){ ros::init(argc, argv, "laser"); ros::NodeHandle node; ros::Publisher vis_pub = node.advertise<visualization_msgs::Marker>( "imu", 0 ); visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = ros::Time(); marker.ns = "imu"; marker.id = 0; marker.type = visualization_msgs::Marker::CUBE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = 0; marker.pose.position.y = 0; marker.pose.position.z = 0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; marker.scale.x = 1; marker.scale.y = 0.1; marker.scale.z = 0.1; marker.color.a = 1.0; // Don't forget to set the alpha! marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; tf::TransformBroadcaster br; tf::Transform transform; ros::Rate rate(10.0); tf::Vector3 origin(0.,0.,0.); while (node.ok()){ transform.setOrigin(origin); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); std::cout << "[" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl; br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "laser")); vis_pub.publish( marker ); rate.sleep(); //origin = addToOrigin(origin, tf::Vector3(0.1,0,0)); } return 0; }
//================================================================ // Get current transform of the arm - from torso_lift_link // to the gripper tool frame using tf_listener //================================================================ void biotacArmController::getArmTransform() { bool noTransform = true; ros::Rate rate(1); while (noTransform && ros::ok()) { try { tf_listener->lookupTransform(ReferenceLink, "/l_gripper_tool_frame", ros::Time(0), *store_transform); noTransform = false; } catch (tf::TransformException ex) { ROS_INFO("No valid transform. Trying again"); } rate.sleep(); } }
bool WorldDownloadManager::shiftNear(const Eigen::Affine3f & pose, float distance) { ROS_INFO("kinfu: shiftNear..."); m_kinfu->shiftNear(pose,distance); if (!m_kinfu->isShiftComplete()) // shifting is waiting for cube { ros::Rate rate(10); do { rate.sleep(); m_kinfu->shiftNear(pose,distance); } while (!m_kinfu->isShiftComplete() || ros::isShuttingDown()); } if (ros::isShuttingDown()) return false; return true; }
int main(int argc, char **argv) { //Set up the node and nodehandle. ros::init(argc, argv, "tf_listener"); ros::NodeHandle nh; ROS_INFO_STREAM("Started node tf_listener."); std::string target_frame; std::string source_frame; tf::TransformListener tf_listener; tf::StampedTransform transform; ros::Rate rate(1.); //used to throttle execution if (argc < 3) {//if no arguments are given, use base_link and part as the default frame names target_frame = "/base_link"; source_frame = "/part"; } else {//if there are at least 2 arguments given, use them as frame names target_frame = argv[1]; source_frame = argv[2]; } while (ros::ok()) { try { //find transform from target TO source (naming is a bit confusing), //at the current time (ros::Time()), and assign result to transform. tf_listener.lookupTransform(target_frame, source_frame, ros::Time(), transform); //copy transform to a msg type to utilize stringstream properties //show transform, and distance between two transforms geometry_msgs::Transform buffer; tf::transformTFToMsg(transform, buffer); ROS_INFO_STREAM("Transform from " << target_frame << " to " << source_frame << ": " << std::endl << buffer); ROS_INFO_STREAM("Distance between transforms: " << transform.getOrigin().length() << " meters."); } catch (...) {//assume that the exception thrown is because transform is not available yet ROS_WARN_STREAM("Waiting for transform from " << target_frame << " to " << source_frame); } rate.sleep(); //throttle execution (1 second default) } return 0; }
// Drive a specified distance (based on base odometry information) with given velocity Twist (vector) bool Base::drive(double distance, const geometry_msgs::Twist& velocity) { tf::StampedTransform start_transform; tf::StampedTransform current_transform; // Wait and get transform tf_listener_odom_.waitForTransform("base_footprint", "odom_combined", ros::Time(0), ros::Duration(5.0)); tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), start_transform); // Set cmd_ to velocity and clear angular and linear.z; cmd_ = velocity; cmd_.angular.x = cmd_.angular.y = cmd_.angular.z = cmd_.linear.z = 0; // Loop until pos reached ros::Rate rate(PUBLISH_RATE_); bool done = false; while (!done && nh_.ok()) { // Send the drive command pub_base_vel_.publish(cmd_); rate.sleep(); // Get the current transform try { tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), current_transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); break; } // See how far we've traveled tf::Transform relative_transform = start_transform.inverse() * current_transform; double dist_moved = relative_transform.getOrigin().length(); if (dist_moved >= distance) done = true; } return done; }
int main(int argc,char **argv) { init(); ros::init(argc,argv, "Controller"); ros::NodeHandle nh; ros::Subscriber sub_state = nh.subscribe("auv/state", 1000, &stateListenerCallBack); ros::Subscriber sub_cmd_vel = nh.subscribe("sim/cmd_vel", 1000, &cmd_velCallBack); ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("zeabus/cmd_vel",1000); ros::Rate rate(100); while(ros::ok) { setPreviousState(); ros::spinOnce(); pub.publish(calculatePID()); //printf("Current velo : %lf %lf %lf (%lf %lf %lf)\n",vel[0],vel[1],vel[2],cmd_vel[0],cmd_vel[1],cmd_vel[2]); printf(" x = %lf \n",position[3]); printf("x=%lf y=%lf z=%lf\n",position[0],position[1],position[2]); printf("roll=%lf pitch=%lf yaw=%lf\n",position[3],position[4],position[5]); rate.sleep(); } ros::shutdown(); }
int main(int argc, char** argv) { ros::init(argc, argv, "my_tf_listener"); ROS_INFO("hello ros_info"); ros::NodeHandle node; ros::service::waitForService("spawn"); ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn"); turtlesim::Spawn srv; add_turtle.call(srv); ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 2); tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()) { tf::StampedTransform transform; try { listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } geometry_msgs::Twist vel_msg; vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); turtle_vel.publish(vel_msg); std::cout<<transform.getOrigin().x()<<" "<<transform.getOrigin().y()<<std::endl; //ROS_INFO("%d %d",transform.getOrigin().x(),transform.getOrigin().y()); rate.sleep(); } return 0; };
int main(int argc, char** argv) { ros::init(argc, argv, "displacement"); ros::NodeHandle node; ros::Subscriber sub1 = node.subscribe( "turtle1/pose", 10, &poseCallback1 ); ros::Subscriber sub2 = node.subscribe( "turtle2/pose", 10, &poseCallback2 ); ros::Publisher pub = node.advertise<geometry_msgs::Transform>( "displacement/transform", 1000 ); //Set the loop at 10 HZ ros::Rate rate(10); while (ros::ok()) { ros::spinOnce();//Call this function to process ROS incoming messages. //Calculate the transformation tf::Transform transformTmp; // transformTmp = transform1.inverse() * transform2; tf::Vector3 t1Origin = transform1.getOrigin(); tf::Vector3 t2Origin = transform2.getOrigin(); tf::Quaternion q1 = transform1.getRotation(); tf::Quaternion q2 = transform2.getRotation(); transformTmp.setOrigin( tf::Vector3(t2Origin.getX() - t1Origin.getX(), t2Origin.getY() - t1Origin.getY(), 0.0) ); tf::Quaternion qtemp; qtemp.setRPY(0, 0, q2.getAngle() - q1.getAngle()); transformTmp.setRotation(qtemp); //Publish the transformation geometry_msgs::Transform tg; tf::transformTFToMsg(transformTmp, tg); pub.publish(tg); rate.sleep();//Sleep the rest of the cycle until 10 Hz } return 0; }
void GraspLocalizer::localizeGrasps() { ros::Rate rate(1); std::vector<int> indices(0); while (ros::ok()) { // wait for point clouds to arrive if (num_clouds_received_ == num_clouds_) { // localize grasps if (num_clouds_ > 1) { PointCloud::Ptr cloud(new PointCloud()); *cloud = *cloud_left_ + *cloud_right_; hands_ = localization_->localizeHands(cloud, cloud_left_->size(), indices, false, false); } else { hands_ = localization_->localizeHands(cloud_left_, cloud_left_->size(), indices, false, false); } antipodal_hands_ = localization_->predictAntipodalHands(hands_, svm_file_name_); handles_ = localization_->findHandles(antipodal_hands_, min_inliers_, 0.005); // publish handles grasps_pub_.publish(createGraspsMsg(handles_)); ros::Duration(1.0).sleep(); // publish hands contained in handles grasps_pub_.publish(createGraspsMsgFromHands(handles_)); ros::Duration(1.0).sleep(); // reset num_clouds_received_ = 0; } ros::spinOnce(); rate.sleep(); } }
void TfSubscriber::run() { ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ listener.lookupTransform("map", "base_footprint", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } ROS_INFO("---------------------TF VALUES-----------------------"); ROS_INFO("\tX Value: %.8f", transform.getOrigin().x()); ROS_INFO("\tY Value: %.8f", transform.getOrigin().y()); ROS_INFO("\tZ Value: %.8f", transform.getOrigin().z()); ROS_INFO("-----------------------------------------------------"); rate.sleep(); } }
int main(int argc, char **argv){ ros::init(argc,argv,"pubvel_toggle"); ros::NodeHandle nh; ros::ServiceServer server = nh.advertiseService("toggle_forward",&toggleForward); ros::Publisher pub=nh.advertise<geometry_msgs::Twist>( "turtle1/cmd_vel",1000); ros::Rate rate(2); while(ros::ok()){ geometry_msgs::Twist msg; msg.linear.x = forward?1.0:0.0; msg.angular.z=forward?0.0:1.0; pub.publish(msg); ros::spinOnce(); rate.sleep(); } }
int main(int argc, char** argv) { ros::init(argc, argv, "detector_filter"); ros::NodeHandle node; ros::Publisher pose_pub = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_out", 10); geometry_msgs::PoseWithCovarianceStamped pose; detector::DetectorFilter detector_filter; ros::Rate rate(100.0); while (ros::ok()){ if (detector_filter.getPose(pose)){ pose.header.stamp = ros::Time::now(); pose_pub.publish(pose); } ros::spinOnce(); rate.sleep(); } return 0; }
RPMockHeadTrackingNode::RPMockHeadTrackingNode(ros::NodeHandle& node) : HEAD_COUNT(HEAD_COUNT_DEFAULT), node(node), frame_count(0), depth_image(new cv_bridge::CvImage()) { depth_image->image = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_32F); initializeRandomDepthImage(depth_image); heads_publisher = node.advertise<rp_head_tracking::Heads>("/rp/head_tracking/heads", 1); // Spin at 5 Hz ros::Rate rate(5); while (ros::ok()) { generateAndPublishMockHeads(); ros::spinOnce(); rate.sleep(); } }
main(){ int i,j,p,min,n,area[20],sum,hit[20]; for(i=0;scanf("%d",&n) && n>=0;i++){ for(min=k[i].n=j=0;j<n;j++){ temp[j].get(); if(temp[j]<temp[min])min=j; } temp[101]=temp[min];temp[min]=temp[0];temp[0]=temp[101]; for(p=1;p<n;p++) for(j=2;j<n;j++) if(rate(temp[0],temp[j],temp[j-1])<0){ temp[101]=temp[j];temp[j]=temp[j-1];temp[j-1]=temp[101]; } for(j=0;j<n;j++)k[i].push(temp[j]); area[i]=k[i].areax2(); hit[i]=0; }n=i; while(SCUD.get())for(i=0;i<n;i++)if(k[i].in(SCUD))hit[i]++; for(sum=i=0;i<n;i++)if(hit[i])sum+=area[i]; printf("%d.%d0\n",sum/2,sum%2?5:0); }
int main(int argc, char **argv) { // initialize ros and specifiy node name ros::init(argc, argv, "cob_base_velocity_smoother"); // create Node Class cob_base_velocity_smoother my_velocity_smoother; // get loop rate from class member ros::Rate rate(my_velocity_smoother.getLoopRate()); // actual calculation step with given frequency while(my_velocity_smoother.nh_.ok()){ my_velocity_smoother.calculationStep(); ros::spinOnce(); rate.sleep(); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "frontier_navigation"); ros::NodeHandle nh; ros::NodeHandle* node_ptr = &nh; ros::Rate rate(10); Frontier_Navigation frontier_navigation(node_ptr); ros::Subscriber map = nh.subscribe("map", 1000, &Frontier_Navigation::mapCallback, &frontier_navigation); ros::Subscriber robot_position = nh.subscribe("sb_navigation/robot_position", 1000, &Frontier_Navigation::posCallback, &frontier_navigation); ros::Subscriber cmd_vel = nh.subscribe("cmd_vel", 1000, &Frontier_Navigation::cmdVelCallback, &frontier_navigation); ros::Subscriber goalStatus = nh.subscribe("sb_navigation/status", 1000, &Frontier_Navigation::goalStatusCallback, &frontier_navigation); // ros::Subscriber filteredMap = nh.subscribe("filteredMap", 1000, &Frontier_Navigation::fileredMapCallback, &f_navigation); while(ros::ok()) { ros::spinOnce(); rate.sleep(); } return 0; }
bool testPublishStatus(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Preparing to publish some test statuses"); ros::NodeHandle n; ros::Rate rate(1); hackathon_scheduler::TaskStatus status; status.taskName="test"; status.status="executing"; for (int i=0; i<3; i++) { char buf[10]; sprintf(buf,"%i",i); status.message=buf; taskStatusPublisher->publish(status); rate.sleep(); } status.status="success"; status.message="finished"; taskStatusPublisher->publish(status); ros::spinOnce(); rate.sleep(); return true; }
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ listener.lookupTransform("/0", "/world", ros::Time(0), transform); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); } rate.sleep(); } return 0; };
void SlamNode::run(void) { ros::Time lastMap = ros::Time::now(); ros::Duration durLastMap = ros::Duration(_gridPublishInterval); ros::Rate rate(_loopRate); std::cout << __PRETTY_FUNCTION__ << " waiting for first laser scan to initialize node...\n"; while(ros::ok()) { ros::spinOnce(); if(_initialized) { ros::Time curTime = ros::Time::now(); if((curTime-lastMap).toSec()>durLastMap.toSec()) { _threadGrid->unblock(); lastMap = ros::Time::now(); } } rate.sleep(); } }
int main() { int num, ch, count; float sum = 0; float tax, gtot; // clrscr(); printf("\nWelcome to SEED's grocery store!....\n\n"); list(); // Display a list of available products for user to select. printf("\n\nHow many pruducts would you like to purchase: "); scanf("%d", &num); // count for number of products user wants to purchase count = num; // count retains the value of num provided by user printf("\n\t\t\t\t\t\tYour Bill!"); printf("\n\t\t\t\t========================================="); printf("\nSelect products! \t\tSelected Products:\t\tRate. "); // format for bill generation printf("\n================\t\t=================\t\t====\n"); do { // clrscr(); // list(); printf("\nSelect product number %d: ", count - (num - 1)); // Prompt user to select the next product. scanf("%d", &ch); sum = sum + rate(ch, count - (num - 1)); // total is calculated here. // rate(int, int) returns the price of the current user selected product // which is iteratively added to the current total // count - (num - 1) calculates the serial number of the product, // to be displayed in selected product list num = num - 1; // iterate loop 'num' times } while(num >= 1); // Bill generation and calculation of tax and grand total, could be done in a separate module printf("\n\t\t\t\t========================================="); printf("\n\t\t\t\t\tThe Total is:\t\t%.2f Rs", sum); // Display total tax = 0.14 * sum; // Calculate tax 14% on total, taxrate var can be used instead of hardcoding gtot = sum + tax; // Calculate grand total printf("\n\t\t\t\t\tTax levied:\t\t%.2f Rs", tax); // Display tax, some formatting for proper display printf("\n\n\t\t\t\t\tGrand Total Payable:\t%.2f Rs", gtot); // Display grand total printf("\n\n\n\t\tThank You for shopping! Visit Again!"); getche(); return 0; }
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn"); turtlesim::Spawn srv; add_turtle.call(srv); ros::service::waitForService("spawn"); ros::Publisher turtle_vel = node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10); tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10)); listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); } turtlesim::Velocity vel_msg; vel_msg.angular = 4 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); turtle_vel.publish(vel_msg); rate.sleep(); } return 0; };
RPNavigationNode::RPNavigationNode(ros::NodeHandle& node) : node(node), obstacle_avoidance_driving_direction(STOP), framing_driving_direction(STOP), driving_direction_source(OBSTACLE_AVOIDANCE_NODE) { // Initialize driving direction publisher driving_direction_publisher = node.advertise<std_msgs::UInt16>("/rp/navigation/driving_direction_and_source", 1); // Initialize driving direction subscribers obstacle_avoidance_subscriber = node.subscribe("/rp/obstacle_avoidance/driving_direction", 1, &RPNavigationNode::obstacleAvoidanceDirectionMessageCallback, this); framing_subscriber = node.subscribe("/rp/framing/driving_direction", 1, &RPNavigationNode::framingDirectionMessageCallback, this); // Initialize Command & Control subscriber auton_photography_subscriber = node.subscribe("/rp/autonomous_photography/direction_source", 1, &RPNavigationNode::autonomousPhotographyDirectionSourceMessageCallback, this); // Spin at 5 Hz DrivingDirection driving_direction; DirectionSource direction_source; ros::Rate rate(5); while (ros::ok()) { getDrivingDirectionAndSource(&driving_direction, &direction_source); // Publish the direction ROS_INFO("Navigation direction: %s", (driving_direction == LEFT) ? "LEFT" : (driving_direction == RIGHT) ? "RIGHT" : (driving_direction == STOP) ? "STOP" : (driving_direction == FORWARD) ? "FORWARD" : "NONE"); std_msgs::UInt16 converted_direction_and_source; converted_direction_and_source.data = (drivingDirectionToUint8(driving_direction) << 8) | (directionSourceToUint8(direction_source)); driving_direction_publisher.publish(converted_direction_and_source); ros::spinOnce(); rate.sleep(); } };
int main(int argc, char** argv){ ros::init(argc, argv, "gps_follower"); ros::NodeHandle node; ros::Publisher bot_vel = node.advertise<geometry_msgs::Twist>("husky/cmd_vel", 10); tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()) { tf::StampedTransform transform; try { listener.waitForTransform("/current_goal", "/current_pose", ros::Time(0), ros::Duration(10.0) ); listener.lookupTransform("/current_goal", "/current_pose", ros::Time(0), transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); } geometry_msgs::Twist vel_msg; vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); if(vel_msg.linear.x > 0.8) { vel_msg.linear.x = 0.8; } bot_vel.publish(vel_msg); rate.sleep(); } return 0; };
int main(int argc, char** argv) { /* Init */ printf("Begin init\n"); ros::init(argc, argv, "test_reading_image"); ros::NodeHandle n; ros::Rate rate(33); ImageConverter ic; cv::namedWindow("followme image window, test"); printf("Waiting for ImageConverter\n"); while (!ic.ready) { ros::spinOnce(); rate.sleep(); if (!ros::ok()) { return 0; } } printf("Init OK\n"); /* Main Loop */ while (ros::ok()) { if (ic.ready == true) { cv::imshow("image window, test", ic.curr_image); } ros::spinOnce(); cv::waitKey(3); rate.sleep(); } printf("Bye!\n"); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "cycle_node"); ros::NodeHandle n; ros::ServiceClient client = n.serviceClient<visual_servo_control::request_servo_velocity_vector>("/visual_servo_control_node/request_servo_velocity_vector"); bool first = true; ros::Rate rate(10.0); while (n.ok()) { std::string request_message; if(first) { request_message = "initialize"; visual_servo_control::request_servo_velocity_vector srv; srv.request.request_message = request_message; client.call(srv); first = false; } else { request_message = "cycle"; visual_servo_control::request_servo_velocity_vector srv; srv.request.request_message = request_message; if (client.call(srv)) { std::vector<double> velocity_vector(srv.response.servo_velocity_vector ); ROS_INFO("Velocity vector has been recieved: %f %f %f %f %f %f", velocity_vector[0] , velocity_vector[1] , velocity_vector[2] , velocity_vector[3] , velocity_vector[4] , velocity_vector[5] ); } else { ROS_INFO("Could not call service."); } } ros::spinOnce(); rate.sleep(); } return 0; }
void Coordination::runLoop() { ros::Rate rate(30); while (ros::ok()) { switch (current_state_) { case IDLE: if (!isHandMoving(0.05) && isNearFabric(0.15)) { this->sendTextToRobot("hgrasp"); this->sendRobotGoal(); ROS_ERROR("Human Grabbed Cloth, Waiting for Robot!"); current_state_ = HUMAN; } break; case HUMAN: if (robot_ready_) { ROS_ERROR("Robot Grabbed Cloth, Starting Cooperation!"); this->getCurrentHand(); current_state_ = COOP; } // robot_ready_ = true; // FOR TESTING ONLY!! break; case COOP: if (gripped_) this->sendRobotCurrent(); else { ROS_ERROR("Human Released Cloth, Stopping!"); current_state_ = IDLE; } break; } this->publishFabricVertices(); ros::spinOnce(); rate.sleep(); } }
int main(int argc, char **argv) { if (argc != 7) { std::cerr << "Usage: " << argv[0] << "<topic> <qdot1> <qdot2> <qdot3> <qdot4> <qdot5>" << std::endl; std::cerr << "Units are radians/simulated_time. Time scale to be implemented." << std::endl; exit(0); } std::string topic(argv[1]); ros::init(argc, argv, "setJointVelocity"); ros::NodeHandle nh; ros::Publisher velocity_pub; velocity_pub=nh.advertise<sensor_msgs::JointState>(topic,1); ros::Rate rate(30); double qdot[5]; for (int i=0; i<5; i++) qdot[i]=atof(argv[i+2]); while (ros::ok()) { sensor_msgs::JointState js; js.name.push_back(std::string("q1")); js.velocity.push_back(qdot[0]); js.name.push_back(std::string("q2")); js.velocity.push_back(qdot[1]); js.name.push_back(std::string("q3")); js.velocity.push_back(qdot[2]); js.name.push_back(std::string("q4")); js.velocity.push_back(qdot[3]); js.name.push_back(std::string("q5")); js.velocity.push_back(qdot[4]); velocity_pub.publish(js); ros::spinOnce(); rate.sleep(); } return 0; }