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