GenericDiagnostic(std::string diagnosticName):nh() {
_updater.add(diagnosticName,this, &GenericDiagnostic::nodeDiagnostics);
_timer = nh.createTimer(ros::Duration(4),&GenericDiagnostic::diagnosticTimerCallback,this);
_updater.setHardwareID("none");
};
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);
}
/*!
* \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);
}
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 initialize(UAS &uas,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
diag_updater.add(tdr_diag);
status_pub = nh.advertise<mavros::RadioStatus>("radio_status", 10);
}
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);
}
void initialize(UAS &uas,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
nh.param<std::string>("gps/frame_id", frame_id, "gps");
nh.param<std::string>("gps/time_ref_source", time_ref_source, frame_id);
diag_updater.add(gps_diag);
fix_pub = nh.advertise<sensor_msgs::NavSatFix>("fix", 10);
time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
vel_pub = nh.advertise<geometry_msgs::TwistStamped>("gps_vel", 10);
}
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();
}
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();
}
void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
{
double freq = (double)(count_)/diagnostic_.getPeriod();
if (freq < (.9*desired_freq_))
{
status.summary(2, "Desired frequency not met");
}
else
{
status.summary(0, "Desired frequency met");
}
status.add("Images in interval", count_);
status.add("Desired frequency", desired_freq_);
status.add("Actual frequency", freq);
count_ = 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;
}
/*!
* \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;
}
/*!
* \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::setupDiagnostics()
{
updater_.setHardwareID("Minicrusher 01");
updater_.add("Status", this, &MCDriverNode::checkStatus);
}
void MCDriverNode::updateDiagnostics()
{
updater_.update();
}
void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
{
diagnostic_updater_.update();
}
// 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;
}
void CrioReceiver::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");
}
}
void diagnosticTimerCallback(const ros::TimerEvent&) {
_updater.update();
};
/*
* \brief Updates the diagnostic component. Diagnostics
*
*/
void GuardianPad::Update(){
updater_pad.update();
}
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));
}
void runDiagnostics()
{
self_test_->checkTest();
diagnostic_.update();
}
