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;
}
