C++ (Cpp) pub_rate Examples

Example #1

File: wam_joystick_teleop.cpp Project: brainoutoforder/csir-manipulation

int main(int argc, char** argv)
{
  ros::init(argc, argv, "wam_teleop"); // Start our wam_node and name it "wam_teleop"
  WamTeleop wam_teleop; // Declare a member of WamTeleop "wam_teleop"
  wam_teleop.init(); // Initialize our teleoperation

  ros::Rate pub_rate(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)

  while (wam_teleop.n_.ok()) // Looping at the specified frequency while our ros node is ok
  {
    ros::spinOnce();
    wam_teleop.update(); // Update and send commands to the WAM
    pub_rate.sleep();
  }
  return 0;
}

Example #2

File: wam_node.cpp Project: brainoutoforder/csir-manipulation

int wam_main(int argc, char** argv, ::barrett::ProductManager& pm, ::barrett::systems::Wam<DOF>& wam)
{
  //BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
  ros::init(argc, argv, "wam_node");
  WamNode<DOF> wam_node(wam);
  wam_node.init(pm);
  ros::Rate pub_rate(WamNode<DOF>::PUBLISH_FREQ);
  
  //pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

  while (ros::ok() && pm.getSafetyModule()->getMode() == ::barrett::SafetyModule::ACTIVE)
  {
    ros::spinOnce();
    wam_node.publishWam(pm);
    //wam_node.updateRT(pm);
    pub_rate.sleep();
  }

  return 0;
}

Example #3

File: pr2_teleop_general_joystick.cpp Project: hks95/quad-navigation-drl

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pr2_telep_general_joystick", ros::init_options::NoSigintHandler);
  //signal(SIGINT,quit);

  boost::thread spin_thread(boost::bind(&spin_function));

  Pr2TeleopGeneralJoystick generaljoy;
  generaljoy.init();
  
  ros::Rate pub_rate(FastHz);
    
  unsigned int counter_limit = (unsigned int)(FastHz/SlowHz);

  unsigned int counter = 0;

  ros::Time beforeCall = ros::Time::now();
  ros::Time afterCall = ros::Time::now();
  while (ros::ok()) 
  {
    //ROS_INFO_STREAM("Time since last " << (ros::Time::now()-beforeCall).toSec());
    beforeCall = ros::Time::now();

    if(!generaljoy.gc->isWalkAlongOk() && !generaljoy.set_walk_along_mode_ && !generaljoy.walk_along_init_waiting_) {
      if(generaljoy.convertCurrentVelToDesiredHeadPos(FastHz)) {
        generaljoy.gc->sendHeadCommand(generaljoy.des_pan_pos_, generaljoy.des_tilt_pos_);
      } 
      generaljoy.gc->sendHeadTrackCommand();
      generaljoy.gc->sendBaseCommand(generaljoy.des_vx_, generaljoy.des_vy_, generaljoy.des_vw_);
    }
      
    if((counter % counter_limit) == 0) {
      
      counter = 0;
      
      if(generaljoy.set_walk_along_mode_) {
        bool ok = generaljoy.gc->moveToWalkAlongArmPose();
        //if we didn't get a select while moving the arms
        if(ok && generaljoy.set_walk_along_mode_) {
          ROS_INFO("Arms in walk position");
          generaljoy.walk_along_init_waiting_ = true;
        } else {
          ROS_INFO("Arms not in walk position");
        }
        generaljoy.set_walk_along_mode_ = false;
      }
      
      if(generaljoy.gc->isWalkAlongOk()) {
        
        generaljoy.gc->sendWalkAlongCommand(generaljoy.walk_along_thresh_, 
                                            generaljoy.walk_along_x_dist_max_,
                                            generaljoy.walk_along_x_speed_scale_,
                                            generaljoy.walk_along_y_dist_max_,
                                            generaljoy.walk_along_y_speed_scale_,
                                            generaljoy.walk_along_w_speed_scale_);
      } else {
        if(generaljoy.convertCurrentVelToDesiredTorsoPos(SlowHz)) {
          generaljoy.gc->sendTorsoCommand(generaljoy.des_torso_pos_, generaljoy.des_torso_vel_);
        }
        //generaljoy.gc->updateCurrentWristPositions();
        generaljoy.gc->sendWristVelCommands(generaljoy.des_right_wrist_vel_, generaljoy.des_left_wrist_vel_, SlowHz);
        
        generaljoy.gc->sendArmVelCommands(generaljoy.right_arm_vx_, generaljoy.right_arm_vy_, generaljoy.right_arm_vz_, generaljoy.right_arm_vel_roll_, generaljoy.right_arm_vel_pitch_, generaljoy.right_arm_vel_yaw_,
                                          generaljoy.left_arm_vx_, generaljoy.left_arm_vy_, generaljoy.left_arm_vz_, generaljoy.left_arm_vel_roll_, generaljoy.left_arm_vel_pitch_, generaljoy.left_arm_vel_yaw_,
                                          SlowHz);
      }
    }
    //ROS_INFO_STREAM("Everything took " << (afterCall-beforeCall).toSec());
    counter++;
    pub_rate.sleep();
  }
  
  ros::shutdown();
  spin_thread.join();
  return 0;
}