GenericDiagnostic(std::string diagnosticName):nh() {
		_updater.add(diagnosticName,this, &GenericDiagnostic::nodeDiagnostics);
		_timer = nh.createTimer(ros::Duration(4),&GenericDiagnostic::diagnosticTimerCallback,this);
		_updater.setHardwareID("none");
		
		
	};
Esempio n. 2
0
  void CrioReceiver::setupDiagnostics() {
    updater_.setHardwareID("CRIO-192.168.0.100");	

    updater_.add("Encoders", this, &CrioReceiver::checkEncoderTicks);
    updater_.add("Yaw Sensor", this, &CrioReceiver::checkYawSensor);
    updater_.add("Voltages", this, &CrioReceiver::checkVoltageLevels);
  }
Esempio n. 3
0
		/*!
		* \brief Constructor for SdhNode class
		*
		* \param name Name for the actionlib server
		*/
		SdhNode(std::string name):
			as_(nh_, name, boost::bind(&SdhNode::executeCB, this, _1),true),
			action_name_(name)
		{
			pi_ = 3.1415926;
			// diagnostics
			updater_.setHardwareID(ros::this_node::getName());
			updater_.add("initialization", this, &SdhNode::diag_init);

		}
Esempio n. 4
0
  void initialize(const ros::WallTimerEvent &ignored)
  {
    sub_.setTimeout(ros::Duration(1.0));
    sub_ = swri::Subscriber(nh_, "odom", 100,
                            &SubscriberTest::handleMessage,
                            this);

    diagnostic_updater_.setHardwareID("none");
    diagnostic_updater_.add(
      "swri::Subscriber test (manual diagnostics)", this,
      &SubscriberTest::manualDiagnostics);

    diagnostic_updater_.add(
      "swri::Subscriber test (auto diagnostics)", this,
      &SubscriberTest::autoDiagnostics);
    
    diag_timer_ = nh_.createTimer(ros::Duration(1.0),
                                  &SubscriberTest::handleDiagnosticsTimer,
                                  this);
  }
  ViconReceiver() :
    nh_priv("~"), diag_updater(), min_freq(0.1), max_freq(1000),
        freq_status(diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq)), StreamMode("ClientPullPreFetch"),
        HostName(""), SubjectName(""), SegmentName(""), tf_ref_frame_id("/enu"),
        tf_tracked_frame_id("/pelican1/flyer_vicon"), update_rate(100), vicon_capture_rate(50),
        max_period_between_updates(0), max_abs_jitter(0), lastFrameNumber(0), frameCount(0), droppedFrameCount(0),
        frame_datum(0), latest_time_bias(0), time_bias_reset_count(0), n_markers(0), n_unlabeled_markers(0),
        time_bias_reset(0.05), segment_data_enabled(false), marker_data_enabled(false),
        unlabeled_marker_data_enabled(false), enable_tf_broadcast(false)

  {
    // Diagnostics
    diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
    diag_updater.add(freq_status);
    diag_updater.setHardwareID("none");
    diag_updater.force_update();
    // Parameters
    nh_priv.param("stream_mode", StreamMode, StreamMode);
    nh_priv.param("datastream_hostport", HostName, HostName);
    nh_priv.param("subject_name", SubjectName, SubjectName);
    nh_priv.param("segment_name", SegmentName, SegmentName);
    nh_priv.param("update_rate", update_rate, update_rate);
    nh_priv.param("vicon_capture_rate", vicon_capture_rate, vicon_capture_rate);
    nh_priv.param("tf_ref_frame_id", tf_ref_frame_id, tf_ref_frame_id);
    nh_priv.param("tf_tracked_frame_id", tf_tracked_frame_id, tf_tracked_frame_id);
    nh_priv.param("time_bias_reset", time_bias_reset, time_bias_reset);
    nh_priv.param("enable_tf_broadcast", enable_tf_broadcast, enable_tf_broadcast);
    ROS_ASSERT(init_vicon());
    // Service Server
    ROS_INFO("setting up grab_vicon_pose service server ... ");
    m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose",
                                                                &ViconReceiver::grab_vicon_pose_callback, this);
    // Publishers
    pose_pub = nh_priv.advertise<geometry_msgs::TransformStamped> ("output", 10);
    markers_pub = nh_priv.advertise<vicon_mocap::Markers> ("markers", 10);
    // Timer
    double update_timer_period = 1 / update_rate;
    min_freq = 0.95 * vicon_capture_rate;
    max_freq = 1.05 * vicon_capture_rate;
    updateTimer = nh.createTimer(ros::Duration(update_timer_period), &ViconReceiver::updateCallback, this);
  }
Esempio n. 6
0
  ViconReceiver() :
    nh_priv("~"), diag_updater(), min_freq_(0.1), max_freq_(1000),
        freq_status_(diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_)), stream_mode_("ClientPull"),
        host_name_(""), tf_ref_frame_id_("/world"), tracked_frame_suffix_("vicon"), broadcast_tf_(true),
        lastFrameNumber(0), frameCount(0), droppedFrameCount(0), frame_datum(0), n_markers(0), n_unlabeled_markers(0),
        marker_data_enabled(false), unlabeled_marker_data_enabled(false), grab_frames_(false)

  {
    // Diagnostics
    diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
    diag_updater.add(freq_status_);
    diag_updater.setHardwareID("none");
    diag_updater.force_update();
    // Parameters
    nh_priv.param("stream_mode", stream_mode_, stream_mode_);
    nh_priv.param("datastream_hostport", host_name_, host_name_);
    nh_priv.param("tf_ref_frame_id", tf_ref_frame_id_, tf_ref_frame_id_);
    nh_priv.param("broadcast_tf", broadcast_tf_, broadcast_tf_);
    ROS_ASSERT(init_vicon());
    // Service Server
    ROS_INFO("setting up grab_vicon_pose service server ... ");
    m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose", &ViconReceiver::grabPoseCallback,
                                                                this);

    ROS_INFO("setting up segment calibration service server ... ");
    calibrate_segment_server_ = nh_priv.advertiseService("calibrate_segment", &ViconReceiver::calibrateSegmentCallback,
                                                         this);

    // Publishers
    marker_pub_ = nh.advertise<vicon_bridge::Markers> (tracked_frame_suffix_ + "/markers", 10);

    if (broadcast_tf_)
      tf_broadcaster_ = new tf::TransformBroadcaster();

    startGrabbing();
  }
Esempio n. 7
0
  ///\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;
  }
Esempio n. 8
0
		/*!
		* \brief Constructor for PowercubeChainNode class.
		*
		* \param name Name for the actionlib server.
		*/
		PowercubeChainNode(std::string name):
			as_(n_, name, boost::bind(&PowercubeChainNode::executeCB, this, _1)),
			action_name_(name)
		{
			sem_can_available = false;
			can_sem = SEM_FAILED;

			isInitialized_ = false;
			traj_point_nr_ = 0;
			finished_ = false;

#ifndef SIMU
			PCube_ = new PowerCubeCtrl();
#else
			PCube_ = new simulatedArm();
#endif
			PCubeParams_ = new PowerCubeCtrlParams();

			// implementation of topics to publish
			topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
			topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);

			// implementation of topics to subscribe
			topicSub_DirectCommand_ = n_.subscribe("command", 1, &PowercubeChainNode::topicCallback_DirectCommand, this);

			// implementation of service servers
			srvServer_Init_ = n_.advertiseService("init", &PowercubeChainNode::srvCallback_Init, this);
			srvServer_Stop_ = n_.advertiseService("stop", &PowercubeChainNode::srvCallback_Stop, this);
			srvServer_Recover_ = n_.advertiseService("recover", &PowercubeChainNode::srvCallback_Recover, this);
			srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowercubeChainNode::srvCallback_SetOperationMode, this);

			// implementation of service clients
			//--

			// diagnostics
			updater_.setHardwareID(ros::this_node::getName());
			updater_.add("initialization", this, &PowercubeChainNode::diag_init);


			// read parameters from parameter server
			n_.getParam("CanModule", CanModule_);
			n_.getParam("CanDevice", CanDevice_);
			n_.getParam("CanBaudrate", CanBaudrate_);
			ROS_INFO("CanModule=%s, CanDevice=%d, CanBaudrate=%d",CanModule_.c_str(),CanDevice_,CanBaudrate_);

			// get ModIds from parameter server
			if (n_.hasParam("ModIds"))
			{
				n_.getParam("ModIds", ModIds_param_);
			}
			else
			{
				ROS_ERROR("Parameter ModIds not set");
			}
			ModIds_.resize(ModIds_param_.size());
			for (int i = 0; i<ModIds_param_.size(); i++ )
			{
				ModIds_[i] = (int)ModIds_param_[i];
			}
			std::cout << "ModIds = " << ModIds_param_ << std::endl;
			
			// get JointNames from parameter server
			ROS_INFO("getting JointNames from parameter server");
			if (n_.hasParam("JointNames"))
			{
				n_.getParam("JointNames", JointNames_param_);
			}
			else
			{
				ROS_ERROR("Parameter JointNames not set");
			}
			JointNames_.resize(JointNames_param_.size());
			for (int i = 0; i<JointNames_param_.size(); i++ )
			{
				JointNames_[i] = (std::string)JointNames_param_[i];
			}
			std::cout << "JointNames = " << JointNames_param_ << std::endl;

			PCubeParams_->Init(CanModule_, CanDevice_, CanBaudrate_, ModIds_);
			
			// get MaxAcc from parameter server
			ROS_INFO("getting MaxAcc from parameter server");
			if (n_.hasParam("MaxAcc"))
			{
				n_.getParam("MaxAcc", MaxAcc_param_);
			}
			else
			{
				ROS_ERROR("Parameter MaxAcc not set");
			}
			MaxAcc_.resize(MaxAcc_param_.size());
			for (int i = 0; i<MaxAcc_param_.size(); i++ )
			{
				MaxAcc_[i] = (double)MaxAcc_param_[i];
			}
			PCubeParams_->SetMaxAcc(MaxAcc_);
			std::cout << "MaxAcc = " << MaxAcc_param_ << std::endl;
			
			// load parameter server string for robot/model
			std::string param_name = "robot_description";
			std::string full_param_name;
			std::string xml_string;
			n_.searchParam(param_name,full_param_name);
			n_.getParam(full_param_name.c_str(),xml_string);
			ROS_INFO("full_param_name=%s",full_param_name.c_str());
			if (xml_string.size()==0)
			{
				ROS_ERROR("Unable to load robot model from param server robot_description\n");
				exit(2);
			}
			ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
			
			// extract limits and velocities from urdf model
			urdf::Model model;
			if (!model.initString(xml_string))
			{
				ROS_ERROR("Failed to parse urdf file");
				exit(2);
			}
			ROS_INFO("Successfully parsed urdf file");

			/** @todo: check if yaml parameter file fits to urdf model */

			// get MaxVel out of urdf model
			std::vector<double> MaxVel;
			MaxVel.resize(ModIds_param_.size());
			for (int i = 0; i<ModIds_param_.size(); i++ )
			{
				MaxVel[i] = model.getJoint(JointNames_[i].c_str())->limits->velocity;
				//std::cout << "MaxVel[" << JointNames_[i].c_str() << "] = " << MaxVel[i] << std::endl;
			}
			PCubeParams_->SetMaxVel(MaxVel);
			
			// get LowerLimits out of urdf model
			std::vector<double> LowerLimits;
			LowerLimits.resize(ModIds_param_.size());
			for (int i = 0; i<ModIds_param_.size(); i++ )
			{
				LowerLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->lower;
				std::cout << "LowerLimits[" << JointNames_[i].c_str() << "] = " << LowerLimits[i] << std::endl;
			}
			PCubeParams_->SetLowerLimits(LowerLimits);

			// get UpperLimits out of urdf model
			std::vector<double> UpperLimits;
			UpperLimits.resize(ModIds_param_.size());
			for (int i = 0; i<ModIds_param_.size(); i++ )
			{
				UpperLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->upper;
				std::cout << "UpperLimits[" << JointNames_[i].c_str() << "] = " << UpperLimits[i] << std::endl;
			}
			PCubeParams_->SetUpperLimits(UpperLimits);

			// get UpperLimits out of urdf model
			std::vector<double> Offsets;
			Offsets.resize(ModIds_param_.size());
			for (int i = 0; i<ModIds_param_.size(); i++ )
			{
				Offsets[i] = model.getJoint(JointNames_[i].c_str())->calibration->rising.get()[0];
				std::cout << "Offset[" << JointNames_[i].c_str() << "] = " << Offsets[i] << std::endl;
			}
			PCubeParams_->SetAngleOffsets(Offsets);
			
			cmd_vel_.resize(ModIds_param_.size());
			newvel_ = false;
			can_sem = sem_open("/semcan", O_CREAT, S_IRWXU | S_IRWXG,1);
			if (can_sem != SEM_FAILED)
				sem_can_available = true;
		}
Esempio n. 9
0
  void MCDriverNode::setupDiagnostics()
  {
    updater_.setHardwareID("Minicrusher 01");

    updater_.add("Status", this, &MCDriverNode::checkStatus);
  }
    // Constructor
    NodeClass()
    {
        // initialization of variables
        is_initialized_bool_ = false;
        is_moving_ = false;
        iwatchdog_ = 0;
        last_time_ = ros::Time::now();
        sample_time_ = 0.020;
        x_rob_m_ = 0.0;
        y_rob_m_ = 0.0;
        theta_rob_rad_ = 0.0;
        vel_x_rob_last_ = 0.0;
        vel_y_rob_last_ = 0.0;
        vel_theta_rob_last_ = 0.0;
        // set status of drive chain to WARN by default
        drive_chain_diagnostic_ = diagnostic_status_lookup_.OK; //WARN; <- THATS FOR DEBUGGING ONLY!

        // Parameters are set within the launch file
        // Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
        if (n.hasParam("IniDirectory"))
        {
            n.getParam("IniDirectory", sIniDirectory);
            ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
        }
        else
        {
            sIniDirectory = "Platform/IniFiles/";
            ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
        }

        IniFile iniFile;
        iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
        iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumJoints, true);

        ucar_ctrl_ = new UndercarriageCtrlGeom(sIniDirectory);


        // implementation of topics
        // published topics
        //topic_pub_joint_state_cmd_ = n.advertise<sensor_msgs::JointState>("joint_command", 1);
        topic_pub_controller_joint_command_ = n.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("joint_command", 1);

        topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("odometry", 1);

        // subscribed topics
        topic_sub_CMD_pltf_twist_ = n.subscribe("command", 1, &NodeClass::topicCallbackTwistCmd, this);
        topic_sub_EM_stop_state_ = n.subscribe("/emergency_stop_state", 1, &NodeClass::topicCallbackEMStop, this);
        topic_sub_drive_diagnostic_ = n.subscribe("diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);



        //topic_sub_joint_states_ = n.subscribe("/joint_states", 1, &NodeClass::topicCallbackJointStates, this);
        topic_sub_joint_controller_states_ = n.subscribe("state", 1, &NodeClass::topicCallbackJointControllerStates, this);

        // diagnostics
        updater_.setHardwareID(ros::this_node::getName());
        updater_.add("initialization", this, &NodeClass::diag_init);

        // implementation of service servers
        srvServer_IsMoving = n.advertiseService("is_moving", &NodeClass::srvCallback_IsMoving, this);

        // implementation of service clients
        srv_client_get_joint_state_ = n.serviceClient<cob_base_drive_chain::GetJointState>("GetJointState");

        //set up timer to cyclically call controller-step
        timer_ctrl_step_ = n.createTimer(ros::Duration(sample_time_), &NodeClass::timerCallbackCtrlStep, this);

    }
/*!	\fn SummitControllerClass::SummitControllerClass()
 * 	\brief Public constructor
*/
SummitControllerClass(ros::NodeHandle h) : diagnostic_(),
  node_handle_(h), private_node_handle_("~"), 
  desired_freq_(100),
  freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05)   ),
  command_freq_("Command frequency check", boost::bind(&SummitControllerClass::check_command_subscriber, this, _1))
  {

  ROS_INFO("summit_robot_control_node - Init ");

  // 
  kinematic_modes_ = 1;  
  
  ros::NodeHandle summit_robot_control_node_handle(node_handle_, "summit_robot_control");

  // Get robot model from the parameters
  if (!private_node_handle_.getParam("model", robot_model_)) {
	  ROS_ERROR("Robot model not defined.");
	  exit(-1);
	  }
  else ROS_INFO("Robot Model : %s", robot_model_.c_str());

  // Ackermann configuration - traction - topics
  private_node_handle_.param<std::string>("frw_vel_topic", frw_vel_topic_, "/summit/joint_frw_velocity_controller/command");
  private_node_handle_.param<std::string>("flw_vel_topic", flw_vel_topic_, "/summit/joint_flw_velocity_controller/command");
  private_node_handle_.param<std::string>("blw_vel_topic", blw_vel_topic_, "/summit/joint_blw_velocity_controller/command");
  private_node_handle_.param<std::string>("brw_vel_topic", brw_vel_topic_, "/summit/joint_brw_velocity_controller/command");

  // Ackermann configuration - traction - joint names 
  private_node_handle_.param<std::string>("joint_front_right_wheel", joint_front_right_wheel, "joint_front_right_wheel");
  private_node_handle_.param<std::string>("joint_front_left_wheel", joint_front_left_wheel, "joint_front_left_wheel");
  private_node_handle_.param<std::string>("joint_back_left_wheel", joint_back_left_wheel, "joint_back_left_wheel");
  private_node_handle_.param<std::string>("joint_back_right_wheel", joint_back_right_wheel, "joint_back_right_wheel");

  // Ackermann configuration - direction - topics
  private_node_handle_.param<std::string>("frw_pos_topic", frw_pos_topic_, "/summit/joint_frw_position_controller/command");
  private_node_handle_.param<std::string>("flw_pos_topic", flw_pos_topic_, "/summit/joint_flw_position_controller/command");
  private_node_handle_.param<std::string>("blw_pos_topic", blw_pos_topic_, "/summit/joint_blw_position_controller/command");
  private_node_handle_.param<std::string>("brw_pos_topic", brw_pos_topic_, "/summit/joint_brw_position_controller/command");

  private_node_handle_.param<std::string>("joint_front_right_steer", joint_front_right_steer, "joint_front_right_wheel_dir"); 
  private_node_handle_.param<std::string>("joint_front_left_steer", joint_front_left_steer, "joint_front_left_wheel_dir");
  private_node_handle_.param<std::string>("joint_back_left_steer", joint_back_left_steer, "joint_back_left_wheel_dir");
  private_node_handle_.param<std::string>("joint_back_right_steer", joint_back_right_steer, "joint_back_right_wheel_dir");

  // PTZ topics
  private_node_handle_.param<std::string>("pan_pos_topic", pan_pos_topic_, "/summit/joint_pan_position_controller/command");
  private_node_handle_.param<std::string>("tilt_pos_topic", tilt_pos_topic_, "/summit/joint_tilt_position_controller/command");
  private_node_handle_.param<std::string>("joint_camera_pan", joint_camera_pan, "joint_camera_pan");
  private_node_handle_.param<std::string>("joint_camera_tilt", joint_camera_tilt, "joint_camera_tilt");

  // Robot parameters
  if (!private_node_handle_.getParam("summit_d_wheels", summit_d_wheels_))
        summit_d_wheels_ = SUMMIT_D_WHEELS_M;
  if (!private_node_handle_.getParam("summit_wheel_diameter", summit_wheel_diameter_))
    summit_wheel_diameter_ = SUMMIT_WHEEL_DIAMETER;
  ROS_INFO("summit_d_wheels_ = %5.2f", summit_d_wheels_);
  ROS_INFO("summit_wheel_diameter_ = %5.2f", summit_wheel_diameter_);

  private_node_handle_.param("publish_odom_tf", publish_odom_tf_, true);
  if (publish_odom_tf_) ROS_INFO("PUBLISHING odom->base_footprint tf");
  else ROS_INFO("NOT PUBLISHING odom->base_footprint tf");
  
  // Robot Speeds
  linearSpeedXMps_   = 0.0;
  linearSpeedYMps_   = 0.0;
  angularSpeedRads_  = 0.0;

  // Robot Positions
  robot_pose_px_ = 0.0;
  robot_pose_py_ = 0.0;
  robot_pose_pa_ = 0.0;
  robot_pose_vx_ = 0.0;
  robot_pose_vy_ = 0.0;
  
  // Robot state space control references
  v_ref_ = 0.0;
  alfa_ref_ = 0.0;
  pos_ref_pan_ = 0.0;
  pos_ref_tilt_= 0.0;

  // Imu variables
  ang_vel_x_ = 0.0; ang_vel_y_ = 0.0; ang_vel_z_ = 0.0;
  lin_acc_x_ = 0.0; lin_acc_y_ = 0.0; lin_acc_z_ = 0.0;
  orientation_x_ = 0.0; orientation_y_ = 0.0; orientation_z_ = 0.0; orientation_w_ = 0.0;

  // Default active kinematic mode
  active_kinematic_mode_ = SINGLE_ACKERMANN;

  // Advertise controller services
  srv_SetMode_ = summit_robot_control_node_handle.advertiseService("set_mode", &SummitControllerClass::srvCallback_SetMode, this);
  srv_GetMode_ = summit_robot_control_node_handle.advertiseService("get_mode", &SummitControllerClass::srvCallback_GetMode, this);
  srv_SetOdometry_ = summit_robot_control_node_handle.advertiseService("set_odometry",  &SummitControllerClass::srvCallback_SetOdometry, this);

  // Subscribe to joint states topic
  joint_state_sub_ = summit_robot_control_node_handle.subscribe<sensor_msgs::JointState>("/summit/joint_states", 1, &SummitControllerClass::jointStateCallback, this);

  // Subscribe to imu data
  imu_sub_ = summit_robot_control_node_handle.subscribe("/summit/imu_data", 1, &SummitControllerClass::imuCallback, this);

  // Adevertise reference topics for the controllers 
  ref_vel_frw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( frw_vel_topic_, 50);
  ref_vel_flw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( flw_vel_topic_, 50);
  ref_vel_blw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( blw_vel_topic_, 50);
  ref_vel_brw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( brw_vel_topic_, 50);
  
  ref_pos_frw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( frw_pos_topic_, 50);
  ref_pos_flw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( flw_pos_topic_, 50);
  ref_pos_blw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( blw_pos_topic_, 50);	  
  ref_pos_brw_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( brw_pos_topic_, 50);
  	  
  ref_pos_pan_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( pan_pos_topic_, 50);
  ref_pos_tilt_ = summit_robot_control_node_handle.advertise<std_msgs::Float64>( tilt_pos_topic_, 50);

  // Subscribe to command topic
  cmd_sub_ = summit_robot_control_node_handle.subscribe<ackermann_msgs::AckermannDriveStamped>("command", 1, &SummitControllerClass::commandCallback, this);
  
  // Subscribe to ptz command topic
  ptz_sub_ = summit_robot_control_node_handle.subscribe<robotnik_msgs::ptz>("command_ptz", 1, &SummitControllerClass::command_ptzCallback, this);
  
  // TODO odom topic as parameter
  // Publish odometry 
  odom_pub_ = summit_robot_control_node_handle.advertise<nav_msgs::Odometry>("/summit_robot_control/odom", 1000);

  // Component frequency diagnostics
  diagnostic_.setHardwareID("summit_robot_control - simulation");
  diagnostic_.add( freq_diag_ );
  diagnostic_.add( command_freq_ );
    
  // Topics freq control 
  // For /summit_robot_control/command
  double min_freq = SUMMIT_MIN_COMMAND_REC_FREQ; // If you update these values, the
  double max_freq = SUMMIT_MAX_COMMAND_REC_FREQ; // HeaderlessTopicDiagnostic will use the new values.
  
  subs_command_freq = new diagnostic_updater::HeaderlessTopicDiagnostic("/summit_robot_control/command", diagnostic_,
	                    diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10));
  subs_command_freq->addTask(&command_freq_); // Adding an additional task to the control
  
  // Flag to indicate joint_state has been read
  read_state_ = false;
}
Esempio n. 12
0
  ProsilicaNode(const ros::NodeHandle& node_handle)
    : nh_(node_handle),
      it_(nh_),
      cam_(NULL), running_(false), auto_adjust_stream_bytes_per_second_(false),
      count_(0),
      frames_dropped_total_(0), frames_completed_total_(0),
      frames_dropped_acc_(WINDOW_SIZE),
      frames_completed_acc_(WINDOW_SIZE),
      packets_missed_total_(0), packets_received_total_(0),
      packets_missed_acc_(WINDOW_SIZE),
      packets_received_acc_(WINDOW_SIZE)
  {
    // Two-stage initialization: in the constructor we open the requested camera. Most
    // parameters controlling capture are set and streaming started in configure(), the
    // callback to dynamic_reconfig.
    prosilica::init();

    if (prosilica::numCameras() == 0)
      ROS_WARN("Found no cameras on local subnet");

    // Determine which camera to use. Opening by IP address is preferred, then guid. If both
    // parameters are set we open by IP and verify the guid. If neither are set we default
    // to opening the first available camera.
    ros::NodeHandle local_nh("~");
    unsigned long guid = 0;
    std::string guid_str;
    if (local_nh.getParam("guid", guid_str) && !guid_str.empty())
      guid = strtol(guid_str.c_str(), NULL, 0);

    std::string ip_str;
    if (local_nh.getParam("ip_address", ip_str) && !ip_str.empty()) {
      cam_.reset( new prosilica::Camera(ip_str.c_str()) );

      // Verify guid is the one expected
      unsigned long cam_guid = cam_->guid();
      if (guid != 0 && guid != cam_guid)
        throw prosilica::ProsilicaException(ePvErrBadParameter,
                                            "guid does not match expected");
      guid = cam_guid;
    }
    else {
      if (guid == 0) guid = prosilica::getGuid(0);
      cam_.reset( new prosilica::Camera(guid) );
    }
    hw_id_ = boost::lexical_cast<std::string>(guid);
    ROS_INFO("Found camera, guid = %s", hw_id_.c_str());
    diagnostic_.setHardwareID(hw_id_);

    // Record some attributes of the camera
    tPvUint32 dummy;
    PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &sensor_width_);
    PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &sensor_height_);

    // Try to load intrinsics from on-camera memory.
    loadIntrinsics();

    // Set up self tests and diagnostics.
    // NB: Need to wait until here to construct self_test_, otherwise an exception
    // above from failing to find the camera gives bizarre backtraces
    // (http://answers.ros.org/question/430/trouble-with-prosilica_camera-pvapi).
    self_test_.reset(new self_test::TestRunner);
    self_test_->add( "Info Test", this, &ProsilicaNode::infoTest );
    self_test_->add( "Attribute Test", this, &ProsilicaNode::attributeTest );
    self_test_->add( "Image Test", this, &ProsilicaNode::imageTest );

    diagnostic_.add( "Frequency Status", this, &ProsilicaNode::freqStatus );
    diagnostic_.add( "Frame Statistics", this, &ProsilicaNode::frameStatistics );
    diagnostic_.add( "Packet Statistics", this, &ProsilicaNode::packetStatistics );
    diagnostic_.add( "Packet Error Status", this, &ProsilicaNode::packetErrorStatus );

    diagnostic_timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&ProsilicaNode::runDiagnostics, this));

    // Service call for setting calibration.
    set_camera_info_srv_ = nh_.advertiseService("set_camera_info", &ProsilicaNode::setCameraInfo, this);

    // Start dynamic_reconfigure
    reconfigure_server_.setCallback(boost::bind(&ProsilicaNode::configure, this, _1, _2));
  }