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); }
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(); }
///\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; }