C++ Timer::start方法代码示例

bool checkCopter(double copter_x, double copter_y, double copter_z, 
					double x, double y, double z, char &colour){
	// Initially red. For when the QC is far away.
	colour = 'r';

	// Region when marker should turn yellow. When the QC is reaching the tapping vicinity.
	double markerZone_height = z + (Radius/2) + 0.0625;
	double markerZone_bottom_x = x - Radius*2;
	double markerZone_top_x = x + Radius*2;
	double markerZone_bottom_y = y - Radius*2;
	double markerZone_top_y = y + Radius*2;

	//This uses one tenth of the radius. If it seems to be too much, raise the "10" below.
	//The radius does need to be present otherwise it may not detect the copter every time.
	double roomba_height = z + (Radius/2) + 0.0625;

	//The x coordinates that the copter has to be in
	double bottom_x = x - Radius*2;
	double top_x = x + Radius*2;

	//The y coordinates that the copter has to be in
	double bottom_y = y - Radius*2;
	double top_y = y + Radius*2;

	//If the copter coordinates are (0,0,0), something is probably wrong
	if (copter_x == 0 && copter_y == 0 && copter_z == 0) return false;

	// If the copter is close to the marker.
	if(markerZone_bottom_x <= copter_x && copter_x <= markerZone_top_x){
		if(markerZone_bottom_y <= copter_y && copter_y <= markerZone_top_y){
			if(copter_z <= markerZone_height){
				colour = 'y';
			}
		}
	}
	
	//If the copter is within the radius, return true
	if (bottom_x <= copter_x && copter_x <= top_x){
		if (bottom_y <= copter_y && copter_y <= top_y){
			if((copter_z - 0.182) <= roomba_height){ // 0.182 is the length between copter centre of mass and the tip of its legs (base)
				timer_5.stop();
				timer_20.stop();
				timer_5.start();
				timer_20.start();
				count_5 = looprate*3; // 3 so it never goes lower than limit
				count_20 = looprate*3;
				colour = 'g';
				return true;
			}
		}
	}
	return false;
}

    void connection_cb(bool connected) {
        // if connection changes, start delayed version request
        version_retries = RETRIES_COUNT;
        if (connected)
            autopilot_version_timer.start();
        else
            autopilot_version_timer.stop();

        // add/remove APM diag tasks
        if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
            UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
            UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
            ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
                           "Extra diagnostic disabled.");
#else
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
        }
        else {
            UAS_DIAG(uas).removeByName(mem_diag.getName());
            UAS_DIAG(uas).removeByName(hwst_diag.getName());
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
        }
    }

    void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        if (!uas->is_my_target(sysid)) {
            ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
            return;
        }

        mavlink_heartbeat_t hb;
        mavlink_msg_heartbeat_decode(msg, &hb);

        // update context && setup connection timeout
        uas->update_heartbeat(hb.type, hb.autopilot);
        uas->update_connection_status(true);
        timeout_timer.stop();
        timeout_timer.start();

        // build state message after updating uas
        auto state_msg = boost::make_shared<mavros::State>();
        state_msg->header.stamp = ros::Time::now();
        state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
        state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
        state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);

        state_pub.publish(state_msg);
        hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
    }

    FollowerFSM() : hold(true),
        currentState(STOPPED),
        nextState(STOPPED),
        nPriv("~")


    {
        //hold = true;
        //currentState = STOPPED;
        //nextState = STOPPED;

        nPriv.param<std::string>("world_frame", world_frame, "world_lol");
        nPriv.param<std::string>("robot_frame", robot_frame, "robot");
        nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
        nPriv.param<double>("minimum_distance", minimum_distance, 2);
        nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
        nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
        nPriv.param("distance_to_target", distance_to_target, 1.5);
        nPriv.param<double>("kv", kv, 0.03);
        nPriv.param<double>("kalfa", kalfa, 0.08);
        //nPriv.param<double>("rate", frequency, 10);
        rate = new ros::Rate(10.0);

        nPriv.param<std::string>("control_type", cType, "planner");

        if(cType == "planner")
        { ac = new MoveBaseClient("move_base", true);}

        notReceivedNumber = 0;
        sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
        cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);

        timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
        timer.start();
    }

/** 
 * Callback
 * initialisation de la liste des publishers inactif pendant une periode T
 */
void init_caplist(const ros::TimerEvent& ){
   ROS_INFO("Initialisation des Publishers.......!");
	timer2.stop();
	for(int i=0; i<(int)CAP_LIST.size()+1; i++){
	chatter_pub[i].shutdown();	
	}
	//ros::spinOnce();
	timer2.start();

   ROS_INFO("Initialisation terminee *");
}

void setVelocity(double linearVel, double angularVel) 
{
  // Stopping and starting the timer causes it to start counting from 0 again.
  // As long as this is called before the kill swith timer reaches killSwitchTimeout seconds
  // the rover's kill switch wont be called.
  killSwitchTimer.stop();
  killSwitchTimer.start();
  
  velocity.linear.x = linearVel * 1.5;
  velocity.angular.z = angularVel * 8; //scaling factor for sim; removed by aBridge node
  velocityPublish.publish(velocity);
}

void RosAriaNode::sonarConnectCb()
{
  if (!robot->tryLock()) {
    ROS_ERROR("Skipping sonarConnectCb because could not lock");
    return;
  }
  if (!robot->areSonarsEnabled())
  {
    robot->enableSonar();
    sonar_tf_timer.start();
  }
  robot->unlock();
}

  void openCamera(ros::Timer &timer)
  {
      timer.stop();
      while (!camera_)
      {
        try
        {
          boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
          camera_ = boost::make_shared<PMDCamboardNano>(device_serial, plugin_dir, source_plugin, process_plugin);
          device_serial = camera_->getSerialNumber().c_str();
          updater.setHardwareIDf("%s", device_serial.c_str());
          NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str());
          loadCalibrationData();
          NODELET_INFO("Loaded calibration data");
          camera_->update();

          camera_info_ = camera_->getCameraInfo();
        }
        catch (PMDCameraNotOpenedException& e)
        {
          camera_state_ = CAMERA_NOT_FOUND;
          if (device_serial != "")
          {
            std::stringstream err;
            err << "Unable to open PMD camera with serial number " << device_serial;
            state_info_ = err.str();
            NODELET_INFO("%s",state_info_.c_str());
          }
          else
          {
              state_info_ = "Unable to open PMD camera..";
              NODELET_INFO("%s",state_info_.c_str());
          }
          boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
          camera_.reset();
        }
        updater.update();
        boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period));
      }
      timer.start();
  }

		/********** callback for the cmd velocity from the autonomy **********/
		void cmd_vel_callback(const geometry_msgs::Twist& msg)
		{
			watchdogTimer.stop();
			
			error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
			//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
			//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
			error_yaw = msg.angular.z - body_vel.angular.z;
			//std::cout << "error yaw: " << error_yaw << std::endl;
			
			// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
			if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
			{	
				errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
				//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
				
				errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
				//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
				velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
				velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
				velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
				velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
			}
			
			last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
			last_error = error;
			last_error_yaw = error_yaw;
			
			error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
			errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
			error_pub.publish(error_gm);
			errorDot_pub.publish(errorDot_gm);
			
			if (start_autonomous)
			{
				recieved_command_from_tracking = true;
			}
			
			watchdogTimer.start();
		}

 // Callback for estimator
 void featureCB(const aruco_ros::CenterConstPtr& center)
 {
     // Disregard erroneous tag tracks
     if (markerID.compare(center->header.frame_id) != 0)
     {
         return;
     }
     
     // Switching
     if (artificialSwitching)
     {
         if (!estimatorOn)
         {
             return;
         }
     }
     else
     {
         // Feature in FOV
         watchdogTimer.stop();
         estimatorOn = true;
     }
     
     // Time
     ros::Time timeStamp = center->header.stamp;
     double timeNow = timeStamp.toSec();
     double delT = timeNow - lastImageTime;
     lastImageTime = timeNow;
     
     // Object trans w.r.t. image frame, for ground truth
     Vector3d trans;
     tf::StampedTransform transform;
     tfl.waitForTransform("image","ugv0",timeStamp,ros::Duration(0.1));
     tfl.lookupTransform("image","ugv0",timeStamp,transform);
     tf::Vector3 temp_trans = transform.getOrigin();
     trans << temp_trans.getX(),temp_trans.getY(),temp_trans.getZ();
     
     // Object pose w.r.t. image frame
     if (deadReckoning)
     {
         tfl.waitForTransform("image",string("marker")+markerID,timeStamp,ros::Duration(0.1));
         tfl.lookupTransform("image",string("marker")+markerID,timeStamp,transform);
         
         try
         {
             // Additional transforms for predictor
             tf::StampedTransform tfWorld2Marker;
             tf::StampedTransform tfMarker2Odom;
             tfl.waitForTransform("world",string("marker")+markerID,timeStamp,ros::Duration(0.1));
             tfl.lookupTransform("world",string("marker")+markerID,timeStamp,tfWorld2Marker);
             tfl.waitForTransform("ugv0/base_footprint","ugv0/odom",timeStamp,ros::Duration(0.1));
             tfl.lookupTransform("ugv0/base_footprint","ugv0/odom",timeStamp,tfMarker2Odom);
             
             // Save transform
             tf::Quaternion temp_quat = tfWorld2Marker.getRotation();
             Quaterniond qW2M = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
             temp_quat = tfMarker2Odom.getRotation();
             Quaterniond qM2O = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
             qWorld2Odom = qW2M*qM2O;
         }
         catch (tf::TransformException e)
         {
         }
     }
     // else, use quaternion from image to ugv0 transform
     tf::Quaternion temp_quat = transform.getRotation();
     Quaterniond quat = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
     
     // Undistort image coordinates. Returns normalized Euclidean coordinates
     double ptsArray[2] = {center->x,center->y};
     cv::Mat pts(1,1,CV_64FC2,ptsArray);
     cv::Mat undistPts;
     cv::undistortPoints(pts,undistPts,camMat,distCoeffs);
     Vector3d x;
     x << undistPts.at<double>(0,0),undistPts.at<double>(0,1),1/trans(2);
     
     // Target velocities expressed in camera coordinates
     Vector3d vTc = quat*vTt;
     
     // Observer velocities
     Vector3d b = vTc - vCc;
     Vector3d w = -wGCc; //Vector3d::Zero();
     
     // EKF update
     Matrix<double,3,2> K = P*H.transpose()*(H*P*H.transpose()+R).inverse();
     xhat += K*(x.head<2>()-xhat.head<2>());
     P = (Matrix3d::Identity()-K*H)*P;
     
     // Publish output
     publishOutput(x,xhat,trans,timeStamp);
     
     if (!artificialSwitching)
     {
         // Restart watchdog timer for feature visibility check
         watchdogTimer.start();
     }
 }

 void CurrentActionListCallback(const supervisor_msgs::ActionsList::ConstPtr& msg)
 {
   try
   {
       if(!msg->actions.empty())
       {
           for(int i = 0 ; i < msg->actions.size() ; ++i)
           {
               for(int j = 0 ; j < msg->actions[i].actors.size() ; ++j)
               {
                   if(msg->actions[i].actors[j] == "HERAKLES_HUMAN1")
                   {
                       ROS_INFO("[robot_observer] Action detected");
                       if(msg->actions[i].focusTarget=="RED_CUBE"){
                           task_started_=true;
                           current_action_position_=red_cube_position_;
                           current_action_=msg->actions[i];
                           previous_action_=msg->actions[i];
                           object_position_=current_action_position_;
                           enable_event_=false;
                           waiting_timer_.setPeriod(ros::Duration(1.0));
                           waiting_timer_.start();
                           task_started_=true;
                           state_machine_->process_event(humanActing());
                       }
                       if(msg->actions[i].focusTarget=="BLACK_CUBE"){
                           task_started_=true;
                           current_action_position_=black_cube_position_;
                           current_action_=msg->actions[i];
                           previous_action_=msg->actions[i];
                           object_position_=current_action_position_;
                           enable_event_=false;
                           waiting_timer_.setPeriod(ros::Duration(1.0));
                           waiting_timer_.start();
                           task_started_=true;
                           state_machine_->process_event(humanActing());
                       }
                       if(msg->actions[i].focusTarget=="BLUE_CUBE"){
                           task_started_=true;
                           current_action_position_=blue_cube_position_;
                           current_action_=msg->actions[i];
                           previous_action_=msg->actions[i];
                           enable_event_=false;
                           waiting_timer_.setPeriod(ros::Duration(1.0));
                           waiting_timer_.start();
                           task_started_=true;
                           state_machine_->process_event(humanActing());
                       }
                       if(msg->actions[i].focusTarget=="GREEN_CUBE2"){
                           task_started_=true;
                           current_action_position_=green_cube_position_;
                           current_action_=msg->actions[i];
                           previous_action_=msg->actions[i];
                           enable_event_=false;
                           waiting_timer_.setPeriod(ros::Duration(1.0));
                           waiting_timer_.start();
                           task_started_=true;
                           state_machine_->process_event(humanActing());
                       }
                       if(msg->actions[i].focusTarget=="PLACEMAT_RED"){
                           task_started_=true;
                           current_action_position_=placemat_position_;
                           current_action_ =msg->actions[i];
                           previous_action_=msg->actions[i];
                           object_position_=current_action_position_;
                           enable_event_=false;
                           waiting_timer_.setPeriod(ros::Duration(1.0));
                           waiting_timer_.start();
                           task_started_=true;
                           state_machine_->process_event(humanActing());
                       }
                   }
               } 
           }    
       }
   }
   catch (HeadManagerException& e )
   {
     ROS_ERROR("[robot_observer] Exception was caught : %s",e.description().c_str());
   }
 }

  bool grab_vicon_pose_callback(vicon_mocap::viconGrabPose::Request& req, vicon_mocap::viconGrabPose::Response& resp)
  {
    ROS_INFO("Got request for a VICON pose");
    updateTimer.stop();
    tf::StampedTransform transform;
    //tf::Quaternion quat;

    if (not segment_data_enabled)
    {
      MyClient.EnableSegmentData();
      ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
      segment_data_enabled = true;
    }

    // Gather data:
    int N = req.n_measurements;
    double accum_trans[3] = {0, 0, 0};
    double accum_quat[4] = {0, 0, 0, 0};
    Result::Enum the_result;
    int n_success = 0;
    for (int k = 0; k < N; k++)
    {
      ros::Duration(1 / vicon_capture_rate).sleep(); // Wait at least as long as vicon needs to capture the next frame
      if ((the_result = MyClient.GetFrame().Result) == Result::Success)
      {
        try
        {
          Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(req.subject_name,
                                                                                          req.segment_name);
          Output_GetSegmentGlobalRotationQuaternion quat =
              MyClient.GetSegmentGlobalRotationQuaternion(req.subject_name, req.segment_name);
          if ((!trans.Occluded) && (!quat.Occluded))
          {
            accum_trans[0] += trans.Translation[0] / 1000;
            accum_trans[1] += trans.Translation[1] / 1000;
            accum_trans[2] += trans.Translation[2] / 1000;
            accum_quat[0] += quat.Rotation[3];
            accum_quat[1] += quat.Rotation[0];
            accum_quat[2] += quat.Rotation[1];
            accum_quat[3] += quat.Rotation[2];
            n_success++;
          }
        }
        catch (tf::TransformException ex)
        {
          ROS_ERROR("%s", ex.what());
          resp.success = false;
          return false; // TODO: should we really bail here, or just try again?
        }
      }
      else
      {
        if (the_result != Result::NoFrame)
        {
          ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
        }
      }
    }

    ROS_INFO("Averaging %d measurements", n_success);
    // Average the data:
    double normalized_quat[4];
    double quat_norm = sqrt(
                            accum_quat[0] * accum_quat[0] + accum_quat[1] * accum_quat[1] + accum_quat[2]
                                * accum_quat[2] + accum_quat[3] * accum_quat[3]);
    for (int i = 0; i < 4; i++)
    {
      normalized_quat[i] = accum_quat[i] / quat_norm;
    }
    double normalized_vector[3];
    // Copy to inputs:
    for (int i = 0; i < 3; i++)
    {
      normalized_vector[i] = accum_trans[i] / n_success;
    }

    // copy what we used to service call response:
    resp.success = true;
    resp.pose.header.stamp = ros::Time::now();
    resp.pose.header.frame_id = tf_ref_frame_id;
    resp.pose.pose.position.x = normalized_vector[0];
    resp.pose.pose.position.y = normalized_vector[1];
    resp.pose.pose.position.z = normalized_vector[2];
    resp.pose.pose.orientation.w = normalized_quat[0];
    resp.pose.pose.orientation.x = normalized_quat[1];
    resp.pose.pose.orientation.y = normalized_quat[2];
    resp.pose.pose.orientation.z = normalized_quat[3];

    updateTimer.start();
    return true;
  }

int main(int argc, char** argv)
{
	ROS_INFO("Started fullbody_teleop.");
	ros::init(argc, argv, "fullbody_teleop");

	ros::NodeHandle nh;

	std::string robotDescription;
	if (!nh.getParam("/robot_description", robotDescription))
	{
		ROS_FATAL("Parameter for robot description not provided");
	}
	huboModel.initString(robotDescription);

	//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

	gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );

	for (auto jointPair : huboModel.joints_)
	{
		boost::shared_ptr<urdf::Joint> joint = jointPair.second;
		if (joint->name[1] == 'F') { continue; }
		if (joint->name== "TSY") { continue; }
		makeJointMarker(joint->name);
	}
	std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;

	ros::Duration(0.1).sleep();

	for (int i = 0; i < HUBO_JOINT_COUNT; i++)
	{
		if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
		{
			planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
			planState.position.push_back(0);
			planState.velocity.push_back(0);
			planState.effort.push_back(0);
		}
	}

	for (int side = 0; side < 2; side++)
		for (int i = 1; i <= 3; i++)
			for (int j = 1; j <= 3; j++)
			{
				std::string sideStr = (side == 0) ? "R" : "L";
				planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
				planState.position.push_back(0);
				planState.velocity.push_back(0);
				planState.effort.push_back(0);
			}

	makeSaveButton();
	gIntServer->applyChanges();

	gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
	gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
	gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
	gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
	gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
	gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);

	gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
	gTimer.start();

	ros::spin();

	gIntServer.reset();
	return 0;
}

		/********** callback for the controller **********/
		void joy_callback(const sensor_msgs::Joy& msg)
		{
			watchdogTimer.stop();
			
			command_from_xbox = geometry_msgs::Twist();// cleaning the xbox twist message
			a_button = msg.buttons[0];// a button lands
			if (a_button > 0)
			{
				land_pub.publish(std_msgs::Empty());
			}
			x_button = msg.buttons[2];// x button sends constant
			if (x_button > 0)
			{
				send_0 = true;
			}
			
			b_button = msg.buttons[1];// b button kills motors
			if (b_button > 0)
			{
				reset_pub.publish(std_msgs::Empty());
			}
			
			y_button = msg.buttons[3];// y button takes off
			if (y_button > 0)
			{
				takeoff_pub.publish(std_msgs::Empty());
			}
				
			dpad_l = msg.buttons[11];//dpad left value, fine adjustment of tilt angle negatively
			dpad_r = msg.buttons[12];//dpad right value, fine adjustment of tilt angle positively
			dpad_u = msg.buttons[13];//dpad up value, coarse adjustment of tilt angle positively
			dpad_d = msg.buttons[14];//dpad down value, coarse adjustment of tilt angle negatively
			
			int tilt_neg = -1*dpad_l - 10*dpad_d;// tilt negatively
			int tilt_pos = 1*dpad_r + 10*dpad_u;// tilt positively
			gimbal_state_desired.angular.y = gimbal_state_desired.angular.y + tilt_neg + tilt_pos;// adjust the gimbal
			camera_state_pub.publish(gimbal_state_desired);// update the angle
			
			left_bumper = msg.buttons[4];// left bumper for using the tracking as the controller output
			right_bumper = msg.buttons[5];// right bumper for using the mocap as the controller output
			
			start_autonomous = (left_bumper > 0) || (right_bumper > 0);// start the autonomous if either are greater than 0
			
			right_stick_vert = joy_deadband(msg.axes[4]);// right thumbstick vert controls linear x
			command_from_xbox.linear.x = joy_gain*right_stick_vert;
			
			right_stick_horz = joy_deadband(msg.axes[3]);// right thumbstick horz controls linear y
			command_from_xbox.linear.y = joy_gain*right_stick_horz;
			
			left_stick_vert = joy_deadband(msg.axes[1]);// left thumbstick vert controls linear z
			command_from_xbox.linear.z = joy_gain*left_stick_vert;
			
			left_stick_horz = joy_deadband(msg.axes[0]);// left thumbstick horz controls angular z
			command_from_xbox.angular.z = -1*joy_gain*left_stick_horz;
			
			if (!start_autonomous)// if not using the autonomous velocities as output will indicate recieved a new control input
			{
				if (send_0)
				{
					//command_from_xbox.linear.x = 0;
					//command_from_xbox.linear.y = 0;
					//command_from_xbox.linear.z = 0;
					//command_from_xbox.angular.z = 0;
					send_0 = false;
				}
				recieved_command_from_xbox = true;
			}
			
			watchdogTimer.start();
		}