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