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();
    }
}
Beispiel #2
0
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;
};
Beispiel #4
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();
  }  
}
Beispiel #6
0
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;
}
Beispiel #9
0
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;
  }
Beispiel #12
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();
	}
}
Beispiel #13
0
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();
  }
}
Beispiel #14
0
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();
	}


}
Beispiel #15
0
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();
    }
}
Beispiel #17
0
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;
}
Beispiel #19
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;
}
Beispiel #20
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;
}
Beispiel #21
0
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;
};
Beispiel #22
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();
  }
}
Beispiel #23
0
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;
};
Beispiel #27
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;
}
Beispiel #28
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;
}