void grabThread()
  {
    ros::Duration d(1.0 / 240.0);
//    ros::Time last_time = ros::Time::now();
//    double fps = 100.;
//    ros::Duration diff;
//    std::stringstream time_log;
    while (ros::ok() && grab_frames_)
    {
      while (msvcbridge::GetFrame().Result != Result::Success && ros::ok())
      {
        ROS_INFO("getFrame returned false");
        d.sleep();
      }
      now_time = ros::Time::now();
//      diff = now_time-last_time;
//      fps = 1.0/(0.9/fps + 0.1*diff.toSec());
//      time_log.clear();
//      time_log.str("");
//      time_log <<"timings: dt="<<diff<<" fps=" <<fps;
//      time_log_.push_back(time_log.str());
//      last_time = now_time;

      bool was_new_frame = process_frame();
      ROS_WARN_COND(!was_new_frame, "grab frame returned false");

      diag_updater.update();
    }
  }
bool spin()
{
    ROS_INFO("summit_robot_control::spin()");
    ros::Rate r(desired_freq_);  // 50.0 

    while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
    {
      if (starting() == 0)
      {
	    while(ros::ok() && node_handle_.ok()) {
          UpdateControl();
          UpdateOdometry();
          PublishOdometry();
          diagnostic_.update();
          ros::spinOnce();
	      r.sleep();
          }
	      ROS_INFO("END OF ros::ok() !!!");
      } else {
       // No need for diagnostic here since a broadcast occurs in start
       // when there is an error.
       usleep(1000000);
       ros::spinOnce();
      }
   }

   return true;
}
  void updateCallback(const ros::TimerEvent& e)
  {
    static bool got_first = false;
    latest_dt = (e.current_real - e.last_real).toSec();
    latest_jitter = (e.current_real - e.current_expected).toSec();
    max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
    Result::Enum the_result;
    bool was_new_frame = true;

    if ((not segment_data_enabled) //
        and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
    {
      the_result = MyClient.EnableSegmentData().Result;
      if (the_result != Result::Success)
      {
        ROS_ERROR("Enable segment data call failed");
      }
      else
      {
        ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
        ROS_INFO("Segment data enabled");
        segment_data_enabled = true;
      }
    }

    if (segment_data_enabled)
    {
      while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
        ;
      {
        now_time = ros::Time::now(); // try to grab as close to getting message as possible
        was_new_frame = process_frame();
        got_first = true;
      }

      if (the_result != Result::NoFrame)
      {
        ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
      }

      if (got_first)
      {
        max_period_between_updates = max(max_period_between_updates, latest_dt);
        last_callback_duration = e.profile.last_duration.toSec();
      }
    }
    diag_updater.update();
  }
  ///\brief Opens joystick port, reads from port and publishes while node is active
  int main(int argc, char **argv)
  {
    diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
    diagnostic_.setHardwareID("none");

    // Parameters
    ros::NodeHandle nh_param("~");
    pub_ = nh_.advertise<fmMsgs::Joy>("joy", 1);
    nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
    nh_param.param<double>("deadzone", deadzone_, 0.05);
    nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
    nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
    
    // Checks on parameters
    if (autorepeat_rate_ > 1 / coalesce_interval_)
      ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
    
    if (deadzone_ >= 1)
    {
      ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
      deadzone_ /= 32767;
    }
    
    if (deadzone_ > 0.9)
    {
      ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
      deadzone_ = 0.9;
    }
    
    if (deadzone_ < 0)
    {
      ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
      deadzone_ = 0;
    }

    if (autorepeat_rate_ < 0)
    {
      ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
      autorepeat_rate_ = 0;
    }
    
    if (coalesce_interval_ < 0)
    {
      ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
      coalesce_interval_ = 0;
    }
    
    // Parameter conversions
    double autorepeat_interval = 1 / autorepeat_rate_;
    double scale = -1. / (1. - deadzone_) / 32767.;
    double unscaled_deadzone = 32767. * deadzone_;

    js_event event;
    struct timeval tv;
    fd_set set;
    int joy_fd;
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = ros::Time::now().toSec();
    
    // Big while loop opens, publishes
    while (nh_.ok())
    {                                      
      open_ = false;
      diagnostic_.force_update();
      bool first_fault = true;
      while (true)
      {
        ros::spinOnce();
        if (!nh_.ok())
          goto cleanup;
        joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        if (joy_fd != -1)
        {
          // There seems to be a bug in the driver or something where the
          // initial events that are to define the initial state of the
          // joystick are not the values of the joystick when it was opened
          // but rather the values of the joystick when it was last closed.
          // Opening then closing and opening again is a hack to get more
          // accurate initial state data.
          close(joy_fd);
          joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        }
        if (joy_fd != -1)
          break;
        if (first_fault)
        {
          ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
          first_fault = false;
        }
        sleep(1.0);
        diagnostic_.update();
      }
      
      ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
      open_ = true;
      diagnostic_.force_update();
      
      bool tv_set = false;
      bool publication_pending = false;
      tv.tv_sec = 1;
      tv.tv_usec = 0;
      fmMsgs::Joy joy_msg; // Here because we want to reset it on device close.
      while (nh_.ok()) 
      {
        ros::spinOnce();
        
        bool publish_now = false;
        bool publish_soon = false;
        FD_ZERO(&set);
        FD_SET(joy_fd, &set);
        
        //ROS_INFO("Select...");
        int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
        //ROS_INFO("Tick...");
        if (select_out == -1)
        {
          tv.tv_sec = 0;
          tv.tv_usec = 0;
          //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
          continue;
          //				break; // Joystick is probably closed. Not sure if this case is useful.
        }
        
        if (FD_ISSET(joy_fd, &set))
        {
          if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
            break; // Joystick is probably closed. Definitely occurs.
          
          //ROS_INFO("Read data...");
          joy_msg.header.stamp = ros::Time().now();
          event_count_++;
          switch(event.type)
          {
          case JS_EVENT_BUTTON:
          case JS_EVENT_BUTTON | JS_EVENT_INIT:
            if(event.number >= joy_msg.buttons.size())
            {
              int old_size = joy_msg.buttons.size();
              joy_msg.buttons.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
                joy_msg.buttons[i] = 0.0;
            }
            joy_msg.buttons[event.number] = (event.value ? 1 : 0);
            // For initial events, wait a bit before sending to try to catch
            // all the initial events.
            if (!(event.type & JS_EVENT_INIT))
              publish_now = true;
            else
              publish_soon = true;
            break;
          case JS_EVENT_AXIS:
          case JS_EVENT_AXIS | JS_EVENT_INIT:
            if(event.number >= joy_msg.axes.size())
            {
              int old_size = joy_msg.axes.size();
              joy_msg.axes.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
                joy_msg.axes[i] = 0.0;
            }
            if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
            {
              double val = event.value;
              // Allows deadzone to be "smooth"
              if (val > unscaled_deadzone)
                val -= unscaled_deadzone;
              else if (val < -unscaled_deadzone)
                val += unscaled_deadzone;
              else
                val = 0;
              joy_msg.axes[event.number] = val * scale;
            }
            // Will wait a bit before sending to try to combine events. 				
            publish_soon = true;
            break;
          default:
            ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
            break;
          }
        }
        else if (tv_set) // Assume that the timer has expired.
          publish_now = true;
        
        if (publish_now)
        {
          // Assume that all the JS_EVENT_INIT messages have arrived already.
          // This should be the case as the kernel sends them along as soon as
          // the device opens.
          //ROS_INFO("Publish...");
          pub_.publish(joy_msg);
          publish_now = false;
          tv_set = false;
          publication_pending = false;
          publish_soon = false;
          pub_count_++;
        }
        
        // If an axis event occurred, start a timer to combine with other
        // events.
        if (!publication_pending && publish_soon)
        {
          tv.tv_sec = trunc(coalesce_interval_);
          tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
          publication_pending = true;
          tv_set = true;
          //ROS_INFO("Pub pending...");
        }
        
        // If nothing is going on, start a timer to do autorepeat.
        if (!tv_set && autorepeat_rate_ > 0)
        {
          tv.tv_sec = trunc(autorepeat_interval);
          tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6; 
          tv_set = true;
          //ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
        }
        
        if (!tv_set)
        {
          tv.tv_sec = 1;
          tv.tv_usec = 0;
        }
	
        diagnostic_.update();
      } // End of joystick open loop.
      
      close(joy_fd);
      ros::spinOnce();
      if (nh_.ok())
        ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
    }
    
  cleanup:
    ROS_INFO("joy_node shut down.");
    
    return 0;
  }
		/*!
		* \brief Routine for publishing joint_states.
		*
		* Gets current positions and velocities from the hardware and publishes them as joint_states.
		*/
		void publishJointState()
		{
		  updater_.update();
			if (isInitialized_ == true)
			{
				// publish joint state message
				int DOF = ModIds_param_.size();
				std::vector<double> ActualPos;
				std::vector<double> ActualVel;
				ActualPos.resize(DOF);
				ActualVel.resize(DOF);

				lock_semaphore(can_sem);
				int success = PCube_->getConfig(ActualPos);
				if (!success) return;
				PCube_->getJointVelocities(ActualVel);
				unlock_semaphore(can_sem);

				sensor_msgs::JointState msg;
				msg.header.stamp = ros::Time::now();
				msg.name.resize(DOF);
				msg.position.resize(DOF);
				msg.velocity.resize(DOF);

				msg.name = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					msg.position[i] = ActualPos[i];
					msg.velocity[i] = ActualVel[i];
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
				}

				topicPub_JointState_.publish(msg);

				// publish controller state message
				pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
				controllermsg.header.stamp = ros::Time::now();
				controllermsg.joint_names.resize(DOF);
				controllermsg.desired.positions.resize(DOF);
				controllermsg.desired.velocities.resize(DOF);
				controllermsg.actual.positions.resize(DOF);
				controllermsg.actual.velocities.resize(DOF);
				controllermsg.error.positions.resize(DOF);
				controllermsg.error.velocities.resize(DOF);

				controllermsg.joint_names = JointNames_;

				for (int i = 0; i<DOF; i++ )
				{
					//std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
					
					if (traj_point_.positions.size() != 0)
						controllermsg.desired.positions[i] = traj_point_.positions[i];
					else
						controllermsg.desired.positions[i] = 0.0;
					controllermsg.desired.velocities[i] = 0.0;
					
					controllermsg.actual.positions[i] = ActualPos[i];
					controllermsg.actual.velocities[i] = ActualVel[i];
					
					controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
					controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
				}
				topicPub_ControllerState_.publish(controllermsg);

			}
		}
 void MCDriverNode::updateDiagnostics()
 {
   updater_.update();
 }
Exemple #7
0
	/*!
	* \brief Main routine to update sdh.
	*
	* Sends target to hardware and reads out current configuration.
	*/
	void updateSdh()
	{
		ROS_DEBUG("updateJointState");
		updater_.update();
		if (isInitialized_ == true)
		{
			if (hasNewGoal_ == true)
			{
				// stop sdh first when new goal arrived
				try
				{
					sdh_->Stop();
				}
				catch (SDH::cSDHLibraryException* e)
				{
					ROS_ERROR("An exception was caught: %s", e->what());
					delete e;
				}
		
				std::string operationMode;
				nh_.getParam("OperationMode", operationMode);
				if (operationMode == "position")
				{
					ROS_DEBUG("moving sdh in position mode");

					try
					{
						sdh_->SetAxisTargetAngle( axes_, targetAngles_ );
						sdh_->MoveHand(false);
					}
					catch (SDH::cSDHLibraryException* e)
					{
						ROS_ERROR("An exception was caught: %s", e->what());
						delete e;
					}
				}
				else if (operationMode == "velocity")
				{
					ROS_DEBUG("moving sdh in velocity mode");
					//sdh_->MoveVel(goal->trajectory.points[0].velocities);
					ROS_WARN("Moving in velocity mode currently disabled");
				}
				else if (operationMode == "effort")
				{
					ROS_DEBUG("moving sdh in effort mode");
					//sdh_->MoveVel(goal->trajectory.points[0].velocities);
					ROS_WARN("Moving in effort mode currently disabled");
				}
				else
				{
					ROS_ERROR("sdh neither in position nor in velocity nor in effort mode. OperationMode = [%s]", operationMode.c_str());
				}
				
				hasNewGoal_ = false;
			}

	 		// read and publish joint angles and velocities
			std::vector<double> actualAngles;
			try
			{
				actualAngles = sdh_->GetAxisActualAngle( axes_ );
			}
			catch (SDH::cSDHLibraryException* e)
			{
				ROS_ERROR("An exception was caught: %s", e->what());
				delete e;
			}
			std::vector<double> actualVelocities;
			try
			{
				actualVelocities = sdh_->GetAxisActualVelocity( axes_ );
			}
			catch (SDH::cSDHLibraryException* e)
			{
				ROS_ERROR("An exception was caught: %s", e->what());
				delete e;
			}
			
			ROS_DEBUG("received %d angles from sdh",actualAngles.size());
			
			// create joint_state message
			sensor_msgs::JointState msg;
			msg.header.stamp = ros::Time::now();
			msg.name.resize(DOF_);
			msg.position.resize(DOF_);
			msg.velocity.resize(DOF_);

			// set joint names and map them to angles
			msg.name = JointNames_;
			//['sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']
			// pos
			msg.position[0] = actualAngles[3]*pi_/180.0; // sdh_thumb_2_joint
			msg.position[1] = actualAngles[4]*pi_/180.0; // sdh_thumb_3_joint
			msg.position[2] = actualAngles[0]*pi_/180.0; // sdh_knuckle_joint
			msg.position[3] = actualAngles[5]*pi_/180.0; // sdh_finger_12_joint
			msg.position[4] = actualAngles[6]*pi_/180.0; // sdh_finger_13_joint
			msg.position[5] = actualAngles[1]*pi_/180.0; // sdh_finger_22_joint
			msg.position[6] = actualAngles[2]*pi_/180.0; // sdh_finger_23_joint
			// vel			
			msg.velocity[0] = actualVelocities[3]*pi_/180.0; // sdh_thumb_2_joint
			msg.velocity[1] = actualVelocities[4]*pi_/180.0; // sdh_thumb_3_joint
			msg.velocity[2] = actualVelocities[0]*pi_/180.0; // sdh_knuckle_joint
			msg.velocity[3] = actualVelocities[5]*pi_/180.0; // sdh_finger_12_joint
			msg.velocity[4] = actualVelocities[6]*pi_/180.0; // sdh_finger_13_joint
			msg.velocity[5] = actualVelocities[1]*pi_/180.0; // sdh_finger_22_joint
			msg.velocity[6] = actualVelocities[2]*pi_/180.0; // sdh_finger_23_joint

			// publish message
			topicPub_JointState_.publish(msg); 

			// read sdh status
			state_ = sdh_->GetAxisActualState(axes_);
		}
		else
		{
			ROS_DEBUG("sdh not initialized");
		}
	}
/*
 *	\brief Updates the diagnostic component. Diagnostics
 *
 */
void GuardianPad::Update(){
	updater_pad.update();
}
 void runDiagnostics()
 {
   self_test_->checkTest();
   diagnostic_.update();
 }
	void diagnosticTimerCallback(const ros::TimerEvent&) {
		
		_updater.update();
	};
 void CrioReceiver::updateDiagnostics() {
   updater_.update();
 }
 void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
 {
   diagnostic_updater_.update();
 }