//given a month, this function returns the date of the start of the next month
boost::gregorian::date student::startOfNextMonth(boost::gregorian::date d){
boost::gregorian::date eom = d.end_of_month();
boost::gregorian::date_duration dur(1);
boost::gregorian::date nextMonth = eom + dur;
return nextMonth;
}
void renderHierarchicalTrajectory(
robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
ros::NodeHandle& node_handle, robot_model::RobotModelPtr& robot_model)
{
static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
visualization_msgs::MarkerArray>("itomp_planner/trajectory", 10);
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model->getLinkModelNames();
std_msgs::ColorRGBA color;
std::string ns = "robot";
ros::Duration dur(100.0);
std::map<std::string, std_msgs::ColorRGBA> colorMap;
color.a = 0.3;
color.r = 0.5;
color.g = 1.0;
color.b = 1.0;
colorMap["lower_body"] = color;
color.a = 0.3;
color.r = 0.5;
color.g = 1.0;
color.b = 0.3;
colorMap["torso"] = color;
color.g = 0.3;
color.b = 1.0;
colorMap["head"] = color;
color.r = 0.9;
color.g = 0.5;
color.b = 0.3;
colorMap["left_arm"] = color;
color.r = 0.9;
color.g = 0.3;
color.b = 0.5;
colorMap["right_arm"] = color;
color.r = 1.0;
color.g = 1.0;
color.b = 1.0;
colorMap["object"] = color;
const robot_state::JointModelGroup* joint_model_group;
std::map<std::string, std::vector<std::string> > group_links_map;
group_links_map["lower_body"] = robot_model->getJointModelGroup(
"lower_body")->getLinkModelNames();
group_links_map["torso"] =
robot_model->getJointModelGroup("torso")->getLinkModelNames();
group_links_map["head"] =
robot_model->getJointModelGroup("head")->getLinkModelNames();
group_links_map["left_arm"] =
robot_model->getJointModelGroup("left_arm")->getLinkModelNames();
group_links_map["right_arm"] =
robot_model->getJointModelGroup("right_arm")->getLinkModelNames();
group_links_map["object"].clear();
if (robot_model->hasLinkModel("right_hand_object_link"))
group_links_map["object"].push_back("right_hand_object_link");
int num_waypoints = robot_trajectory->getWayPointCount();
for (int i = 0; i < num_waypoints; ++i)
{
ma.markers.clear();
robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr(i);
for (std::map<std::string, std::vector<std::string> >::iterator it =
group_links_map.begin(); it != group_links_map.end(); ++it)
{
std::string ns = "robot_" + it->first;
state->getRobotMarkers(ma, group_links_map[it->first],
colorMap[it->first], ns, dur);
}
vis_marker_array_publisher_.publish(ma);
double time = (i == 0 || i == num_waypoints - 1) ? 2.0 : 0.05;
ros::WallDuration timer(time);
timer.sleep();
}
for (int i = 0; i < 10; ++i)
{
ma.markers.clear();
robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr(
num_waypoints - 1);
for (std::map<std::string, std::vector<std::string> >::iterator it =
group_links_map.begin(); it != group_links_map.end(); ++it)
{
std::string ns = "robot_" + it->first;
state->getRobotMarkers(ma, group_links_map[it->first],
colorMap[it->first], ns, dur);
}
vis_marker_array_publisher_.publish(ma);
double time = 0.05;
ros::WallDuration timer(time);
timer.sleep();
}
}
Vib& set(
float a, float b, float c, float d, float e, float f, float g, float h, float i, ArrayPow2<float>& j, float k
){
return dur(a).freq(b).amp(c).attack(d).decay(e)
.vibRate1(f).vibRate2(g).vibRise(h).vibDepth(i).table(j).pan(k);
}
SineEnv& set(
float a, float b, float c, float d, float e, float f=0
){
return dur(a).freq(b).amp(c).attack(d).decay(e).pan(f);
}
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
if (hectorDrawings)
{
hectorDrawings->setTime(scan.header.stamp);
}
ros::WallTime startTime = ros::WallTime::now();
if (!p_use_tf_scan_transformation_)
{
if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
{
slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
}
}
else
{
ros::Duration dur (0.5);
if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur))
{
tf::StampedTransform laserTransform;
tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);
//projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);
projector_.projectLaser(scan, laser_point_cloud_,30.0);
if (scan_point_cloud_publisher_.getNumSubscribers() > 0){
scan_point_cloud_publisher_.publish(laser_point_cloud_);
}
Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap()))
{
if (initial_pose_set_){
initial_pose_set_ = false;
startEstimate = initial_pose_;
}else if (p_use_tf_pose_start_estimate_){
try
{
tf::StampedTransform stamped_pose;
tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_map_frame_, p_base_frame_, scan.header.stamp, stamped_pose);
tfScalar yaw, pitch, roll;
stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);
startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
startEstimate = slamProcessor->getLastScanMatchPose();
}
}else{
startEstimate = slamProcessor->getLastScanMatchPose();
}
if (p_map_with_known_poses_){
slamProcessor->update(laserScanContainer, startEstimate, true);
}else{
slamProcessor->update(laserScanContainer, startEstimate);
}
}
}else{
ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
return;
}
}
if (p_timing_output_)
{
ros::WallDuration duration = ros::WallTime::now() - startTime;
ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
}
//If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
if (p_map_with_known_poses_)
{
return;
}
poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);
poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
posePublisher_.publish(poseInfoContainer_.getPoseStamped());
if(p_pub_odometry_)
{
nav_msgs::Odometry tmp;
tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
odometryPublisher_.publish(tmp);
}
if (p_pub_map_odom_transform_)
{
tf::StampedTransform odom_to_base;
try
{
tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what());
odom_to_base.setIdentity();
}
map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.1);
tfB_->sendTransform( tf::StampedTransform (map_to_odom_,tf_expiration, p_map_frame_, p_odom_frame_));
}
if (p_pub_map_scanmatch_transform_){
tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
}
}
TInt CTestStepSDevSoundClientThreadNoCap::DoPlaySimpleTone()
{
INFO_PRINTF1(_L("Testing Simple Tone Playback"));
TInt freq = 1000;
TTimeIntervalMicroSeconds dur(10000000); // play for some period of time, eg 10secs
TUid KInvalidCIUid = KUidCustomInterfaceDevSoundFileBlockLength; // use block length CI
TAny* tempPtr = NULL;
if (iAttemptInitialCI)
{
// if requested we get a CustomInferface() prior to SetClientThreadInfo. The latter
// should then fail since it is called prior to InitializeL
tempPtr = iMMFDevSound->CustomInterface(KInvalidCIUid); // request unknown i/f
if (tempPtr)
{
INFO_PRINTF1(_L("CustomInterface() expected to return NULL but did not!"));
return KErrGeneral;
}
}
TInt err = iMMFDevSound->SetClientThreadInfo(iServerTid);
TInt expectedErr = KErrNone;
#ifdef SYMBIAN_MULTIMEDIA_A3FDEVSOUND
if (iAttemptInitialCI)
{
// characterisation says that SetClientThreadInfo() called 2nd returns an error
// but the old DevSound works differently
expectedErr = KErrNotReady;
}
#endif // SYMBIAN_MULTIMEDIA_A3FDEVSOUND
if (err != expectedErr)
{
// with iAttemptInitialCI we expect KErrNotReady otherwise KErrNone
INFO_PRINTF3(_L("Unexpected return from SetClientThreadInfo(%d, not %d)"), err, expectedErr);
return err==KErrNone ? KErrGeneral : err; // always return a real error
}
// always ask for CI after SetClientThreadInfo(). This should be a NOP, and do no harm
// call just to make sure no Kern-exec-3 or similar occurs
tempPtr = iMMFDevSound->CustomInterface(KInvalidCIUid); // request unknown i/f
if (tempPtr)
{
INFO_PRINTF1(_L("CustomInterface() expected to return NULL but did not!"));
return KErrGeneral;
}
// if iAttemptInitialCI, the SetClientThreadInfo() would have failed with KErrNotReady
// return now
if (iAttemptInitialCI)
{
return KErrNone;
}
//Initialize
TVerdict initOK = TestInitialize(EMMFStateTonePlaying);
if (initOK != EPass)
{
return KErrGeneral;
}
TestSetPriority(KDevSoundPriorityHigh);
TestSetVolume(iMMFDevSound->MaxVolume());
iExpectedValue = KErrInUse;
return TestPlayTone(freq, dur);
}
void NewVizManager::animatePath(const ItompTrajectoryConstPtr& trajectory,
const robot_state::RobotStatePtr& robot_state, bool is_best)
{
if (!is_best)
return;
// marker array
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model_->getMoveitRobotModel()->getLinkModelNames();
std_msgs::ColorRGBA color = colors_[WHITE];
color.a = 1.0;
ros::Duration dur(3600.0);
for (unsigned int point = 0; point < trajectory->getNumPoints(); ++point)
{
ma.markers.clear();
const Eigen::MatrixXd mat = trajectory->getElementTrajectory(
ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
robot_state->setVariablePositions(mat.data());
std::string ns = "frame_" + boost::lexical_cast<std::string>(point);
robot_state->getRobotMarkers(ma, link_names, color, ns, dur);
for (int i = 0; i < ma.markers.size(); ++i)
ma.markers[i].mesh_use_embedded_materials = true;
vis_marker_array_publisher_path_.publish(ma);
}
// MotionPlanning -> Planned Path -> trajectory
moveit_msgs::DisplayTrajectory display_trajectory;
int num_all_joints = robot_state->getVariableCount();
ElementTrajectoryConstPtr joint_trajectory = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT);
robot_trajectory::RobotTrajectoryPtr response_trajectory = boost::make_shared<robot_trajectory::RobotTrajectory>(robot_model_->getMoveitRobotModel(), "");
robot_state::RobotState ks = *robot_state;
std::vector<double> positions(num_all_joints);
double dt = trajectory->getDiscretization();
// TODO:
int num_return_points = joint_trajectory->getNumPoints();
for (std::size_t i = 0; i < num_return_points; ++i)
{
for (std::size_t j = 0; j < num_all_joints; j++)
{
positions[j] = (*joint_trajectory)(i, j);
}
ks.setVariablePositions(&positions[0]);
// TODO: copy vel/acc
ks.update();
response_trajectory->addSuffixWayPoint(ks, dt);
}
moveit_msgs::RobotTrajectory trajectory_msg;
response_trajectory->getRobotTrajectoryMsg(trajectory_msg);
display_trajectory.trajectory.push_back(trajectory_msg);
ros::NodeHandle node_handle;
static ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/itomp_planner/display_planned_path", 1, true);
display_publisher.publish(display_trajectory);
}
Duration Duration::operator*(const Duration value) const {
Duration dur(*this);
dur.m_ms = m_ms * value.m_ms;
return dur;
}
int main(int argc, const char *argv[]) {
options::single<int> number('N', "number", "some number", 0);
options::single<int> number2('k', "kumber", "some other number", 0, {0, 3, 3});
options::map<int, std::vector<std::pair<std::string, int> > > numbers('m', "numbers", "several numbers");
options::map<std::string, std::list<std::pair<std::string, std::string> > > strings('s', "strings", "several strings");
options::single<const char *>cs('c', "cstring", "a c string", "", {"bla", "blubb"});
options::single<std::string>Cs('C', "Cstring", "a C++ string", "", {"bla", "blubb"});
options::container<double> dnums('d', "doubles", "double numbers");
// options::container<const char*, std::list<const char*>> stringl('S', "listString", "list of strings");
options::container<std::string, std::list<std::string>> stringS('X', "liststring", "list of std::strings");
options::single<double> complexDescription('\0', "ComplexDescription", "Pass here the Bremsstrahl-Tagging-Hodoscope-Engineering-Assemply-Rate in Hz", 84.);
options::single<double> moreComplexDescription('\0', "MoreComplexDescription", "very complicated example option with very long explanation to illustrate automatic wrapping in help output when the explanations become very long and would break readability otherwise.", 42.);
options::single<double> evenMoreComplexDescription('\0', "EvenMoreComplexDescription", "very complicated example option with very long explanation containing averylongwordwhichisunbreakableandthustriggersforcefulwordwrappinginaninconvenientplacetokeepthingssomehowatleastabitreadable.", 21.);
options::single<options::postFixedNumber<size_t>> size('\0', "size", "a size");
options::single<std::chrono::system_clock::time_point> date('\0', "date", "a date");
options::single<std::chrono::duration<long>> dur('\0', "dur", "a duration");
options::single<bool> lateOption('\0', "lateOption", "try to book an option late", false);
cs.fForbid(&Cs);
Cs.fForbid(&cs);
options::container<std::chrono::system_clock::time_point> dates('\0', "dates", "list of dates");
options::OptionsForTApplication TApplicationOptions(argv[0]);
options::positional<options::single<float>>posNumber(10, "posnumber", "positional float number", 0);
options::positional<options::container<std::string>>files(20, "files", "positional file list");
options::positional<options::single<const char *>>dest(30, "dest", "positional destination file", "");
options::parser parser("option parsing example");
parser.fRequire(&number);
auto unusedOptions = parser.fParse(argc, argv);
TApplicationOptions.fFinalize(unusedOptions.begin(), unusedOptions.end());
for (auto & unusedOption : unusedOptions) {
std::cout << "unused option : '" << unusedOption << "'" << std::endl;
}
for (auto & num : numbers) {
std::cout << " number '" << num.first << "' is '" << num.second << "'\n";
}
for (auto & str : strings) {
std::cout << " string '" << str.first << "' is '" << str.second << "'\n";
}
for (double & dnum : dnums) {
std::cout << " double number '" << dnum << "'\n";
}
// for (auto & it : stringl) {
// std::cout << " list string '" << it << "'\n";
//}
for (auto & it : stringS) {
std::cout << " list std::string '" << it << "'\n";
}
std::cout << "and the time variable is:";
date.fWriteValue(std::cout);
auto timebuf = std::chrono::system_clock::to_time_t(date);
std::cout << ", that is " << std::ctime(&timebuf);
std::cout << "the duration is: ";
dur.fWriteValue(std::cout);
std::cout << " or " << std::chrono::duration_cast<std::chrono::hours>(dur).count() << " hours \n";
if (lateOption) {
options::single<bool> optionLate('\0', "lateOptionTest", "option booked late", false);
}
return number;
}
TInt XAPlaySessionImpl::load(const TDesC& aURI)
{
TInt retVal;
XAresult xaRes;
XAEngineItf engineItf;
XADynamicSourceItf dynamicSourceItf;
XAboolean required[MAX_NUMBER_INTERFACES];
XAInterfaceID iidArray[MAX_NUMBER_INTERFACES];
XAuint32 noOfInterfaces = 0;
TInt i;
XAmillisecond dur(0);
TPtr8 uriPtr(0,0,0);
TPtr8 mimeTypePtr(0,0,0);
#ifdef USE_VIDEOPLAYERUTILITY
TRAP(m_VPError, mVideoPlayUtil->OpenFileL(_L("C:\\data\\test.3gp")));
if (m_VPError)
return 0;
if(!mActiveSchedulerWait->IsStarted())
mActiveSchedulerWait->Start();
if (m_VPError)
return 0;
mVideoPlayUtil->Prepare();
if(!mActiveSchedulerWait->IsStarted())
mActiveSchedulerWait->Start();
return 0;
#endif
delete mURIName;
mURIName = NULL;
TRAP(retVal, mURIName = HBufC8::NewL(aURI.Length()+1));
RET_ERR_IF_ERR(retVal);
uriPtr.Set(mURIName->Des());
// This has to be done here since we can not destroy the Player
// in the Resource Lost callback.
if (mbMediaPlayerUnrealized) {
if (mMOPlayer) {
(*mMOPlayer)->Destroy(mMOPlayer);
mMOPlayer = NULL;
}
}
//py uri name into local variable
//TODO fix copy issue from 16 bit to 8 bit
uriPtr.Copy(aURI);
//If media player object already exists, just switch source
//using dynamic source interface
if (mMOPlayer && mPlayItf) {
dynamicSourceItf = NULL;
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_DYNAMICSOURCE, &dynamicSourceItf);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
//Setup the data source
//TODO Hard coded locator type
mUri.locatorType = XA_DATALOCATOR_URI;
//append zero terminator to end of URI
mUri.URI = (XAchar*) uriPtr.PtrZ();
//TODO Hard coded locator type
mMime.containerType = XA_CONTAINERTYPE_WAV;
//TODO Hard coded locator type
mMime.formatType = XA_DATAFORMAT_MIME;
mimeTypePtr.Set(mWAVMime->Des());
mMime.mimeType = (XAchar*)mimeTypePtr.Ptr();
mDataSource.pFormat = (void*)&mMime;
mDataSource.pLocator = (void*)&mUri;
xaRes = (*dynamicSourceItf)->SetSource(dynamicSourceItf, &mDataSource);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
}
else { // Create media player object
// Setup the data source
// TODO Hard coded locator type
mUri.locatorType = XA_DATALOCATOR_URI;
//append zero terminator to end of URI
mUri.URI = (XAchar*) uriPtr.PtrZ();
//TODO Hard coded locator type
mMime.containerType = XA_CONTAINERTYPE_WAV;
//TODO Hard coded locator type
mMime.formatType = XA_DATAFORMAT_MIME;
mimeTypePtr.Set(mWAVMime->Des());
mMime.mimeType = (XAchar*)mimeTypePtr.Ptr();
mDataSource.pFormat = (void*)&mMime;
mDataSource.pLocator = (void*)&mUri;
//Setup the audio data sink
mLocatorOutputDevice.locatorType = XA_DATALOCATOR_IODEVICE;
mLocatorOutputDevice.deviceType = 6;
mAudioSink.pLocator = (void*) &mLocatorOutputDevice;
mAudioSink.pFormat = NULL;
//Init arrays required[] and iidArray[]
for (i = 0; i < MAX_NUMBER_INTERFACES; i++) {
required[i] = XA_BOOLEAN_FALSE;
iidArray[i] = XA_IID_NULL;
}
noOfInterfaces = 0;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_SEEK;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_DYNAMICSOURCE;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_METADATAEXTRACTION;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_NOKIALINEARVOLUME;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_NOKIAVOLUMEEXT;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_PREFETCHSTATUS;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_STREAMINFORMATION;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_PLAYBACKRATE;
noOfInterfaces++;
required[noOfInterfaces] = XA_BOOLEAN_FALSE;
iidArray[noOfInterfaces] = XA_IID_VIDEOPOSTPROCESSING;
noOfInterfaces++;
xaRes = (*mEOEngine)->GetInterface(mEOEngine, XA_IID_ENGINE, (void**) &engineItf);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*engineItf)->CreateMediaPlayer(engineItf,
&mMOPlayer,
&mDataSource,
NULL,
&mAudioSink,
&mVideoSink,
NULL,
NULL,
noOfInterfaces,
iidArray,
required);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mMOPlayer)->Realize(mMOPlayer, XA_BOOLEAN_FALSE);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
mbMediaPlayerUnrealized = FALSE;
xaRes = (*mMOPlayer)->RegisterCallback(mMOPlayer, MediaPlayerCallback, (void*)this);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_PLAY, &mPlayItf);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mPlayItf)->RegisterCallback(mPlayItf, PlayItfCallback, (void*)this);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mPlayItf)->SetPositionUpdatePeriod(mPlayItf, (XAmillisecond)KPlayPosUpdatePeriod);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mPlayItf)->SetCallbackEventsMask( mPlayItf,
( XA_PLAYEVENT_HEADATEND |
XA_PLAYEVENT_HEADATNEWPOS |
XA_PLAYEVENT_HEADMOVING )
);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_SEEK, &mSeekItf);
retVal = mapError(xaRes, ETrue);
//Metadata
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_METADATAEXTRACTION, &mMetadataExtItf);
if(mapError(xaRes, ETrue)==KErrNone) {
mbMetadataAvailable = ETrue;
setupALKeyMap(); //done only once at creation of meadia player
}
//volume
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_NOKIALINEARVOLUME, &mNokiaLinearVolumeItf);
if(mapError(xaRes, ETrue)==KErrNone)
mbVolEnabled = ETrue;
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_NOKIAVOLUMEEXT, &mNokiaVolumeExtItf);
if(mapError(xaRes, ETrue)==KErrNone)
mbMuteEnabled = ETrue;
//buffer status
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_PREFETCHSTATUS, &mPrefetchStatusItf);
if(mapError(xaRes, ETrue)==KErrNone) {
mbPrefetchStatusChange = ETrue;
(*mPrefetchStatusItf)->RegisterCallback(mPrefetchStatusItf, PrefetchItfCallback, (void*)this);
(*mPrefetchStatusItf)->SetCallbackEventsMask(mPrefetchStatusItf, XA_PREFETCHEVENT_FILLLEVELCHANGE);
}
//stream information
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_STREAMINFORMATION, &mStreamInformationItf);
if(mapError(xaRes, ETrue)==KErrNone) {
mbStreamInfoAvailable = ETrue;
mParent.cbStreamInformation(ETrue); //indicate first time
(*mStreamInformationItf)->RegisterStreamChangeCallback(mStreamInformationItf, StreamInformationItfCallback, (void*)this);
}
//playback rate
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_PLAYBACKRATE, &mPlaybackRateItf);
if(mapError(xaRes, ETrue)==KErrNone)
mbPlaybackRateItfAvailable = ETrue;
//videopostprocessing
xaRes = (*mMOPlayer)->GetInterface(mMOPlayer, XA_IID_VIDEOPOSTPROCESSING, &mVideoPostProcessingItf);
if(mapError(xaRes, ETrue)==KErrNone)
mbScalable = ETrue;
}
if(mbMetadataAvailable) {
keyMap.clear();
extendedKeyMap.clear();
setupMetaData(); //done every time source is changed
}
else { //send signal for seekable
mParent.cbSeekableChanged(ETrue);
}
mCurPosition = 0;
mParent.cbPositionChanged(mCurPosition);
xaRes = (*mPlayItf)->GetDuration(mPlayItf, &dur);
retVal = mapError(xaRes, ETrue);
RET_ERR_IF_ERR(retVal);
mDuration = dur;
mParent.cbDurationChanged(mDuration);
return retVal;
}
Duration Duration::operator*(const double value) const {
Duration dur(*this);
dur.m_ms = qAbs(m_ms * (qint64)value);
return dur;
}
int main( int argc, char *argv[] ) {
tf::options o;
o.register_option(tf::option("help", "Displays help", false, false, "help", 'h'));
o.register_option(tf::option("loglevel", "Logging level (DEBUG, INFO, WARNING, ERROR)", false, true, "loglevel", 'l'));
o.register_option(tf::option("url", "URL to connect to", true, true, "url", 'u'));
o.register_option(tf::option("rate", "Sending message rate", false, true, "rate", 'r'));
o.register_option(tf::option("count", "Number of messages to send", false, true, "count", 'c'));
try {
o.parse(argc, argv);
} catch (const tf::option_exception &e) {
ERROR_LOG(e.what());
o.printUsage();
return 1;
}
std::string loglevel;
if (o.get("loglevel", loglevel)) {
if (loglevel == "DEBUG") {
LOG_LEVEL(tf::logger::debug);
} else if (loglevel == "INFO") {
LOG_LEVEL(tf::logger::info);
} else if (loglevel == "WARNING") {
LOG_LEVEL(tf::logger::warning);
} else if (loglevel == "ERROR") {
LOG_LEVEL(tf::logger::error);
} else {
ERROR_LOG("Invalid log level");
return 1;
}
} else {
LOG_LEVEL(tf::logger::warning);
}
const size_t count = o.get("count", 1000ul);
const size_t rate = o.get("rate", 100ul);
try {
using TimeType = std::chrono::high_resolution_clock::time_point;
using ResultsType = std::pair<TimeType, TimeType>;
std::vector<ResultsType> m_times(count);
fp::Session::initialise();
if (std::thread::hardware_concurrency() >= 4) {
fp::Session::assign_to_cpu({2, 3});
} else if (std::thread::hardware_concurrency() >= 2) {
fp::Session::assign_to_cpu({0, 1});
}
const std::string url = o.getWithDefault("url", "");
fp::BlockingQueue queue;
auto transport = fp::make_realm_connection(url.c_str(), "");
if (!transport->valid()) {
ERROR_LOG("Failed to create transport");
return 1;
}
uint32_t id = 0;
bool shutdown = false;
fp::MutableMessage sendMsg;
sendMsg.setSubject("TEST.PERF.SOURCE");
sendMsg.addScalarField("id", id);
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::microseconds(1000000) / rate);
INFO_LOG("Sending a message every " << duration.count() << " us");
queue.registerEvent(duration, [&](fp::TimerEvent *event) noexcept {
if (id < count) {
sendMsg.clear();
sendMsg.setSubject("TEST.PERF.SOURCE");
sendMsg.addScalarField("id", id);
m_times[id].first = std::chrono::high_resolution_clock::now();
if (transport->sendMessage(sendMsg) == fp::OK) {
DEBUG_LOG("Message send successfully: " << sendMsg);
} else {
ERROR_LOG("Failed to send message");
exit(1);
}
id++;
}
});
fp::Subscriber subscriber(transport, "TEST.PERF.SINK", [&](const fp::Subscriber *event, const fp::Message *recvMsg) noexcept {
DEBUG_LOG("Received message from sink: " << *recvMsg);
// std::chrono::high_resolution_clock::time_point t = std::chrono::high_resolution_clock::now();
uint32_t recv_id = 0;
if (recvMsg->getScalarField("id", recv_id)) {
DEBUG_LOG("Processing message: " << recv_id);
uint64_t ts = 0;
if (recvMsg->getScalarField("timestamp", ts)) {
std::chrono::microseconds dur(ts);
std::chrono::time_point<std::chrono::high_resolution_clock> t(dur);
m_times[recv_id].second = t;
}
}
if (recv_id >= count - 1) {
shutdown = true;
}
});
queue.addSubscriber(subscriber);
while (!shutdown) {
queue.dispatch();
}
fp::Session::destroy();
std::vector<std::chrono::microseconds> latencies;
latencies.reserve(count);
std::transform(m_times.begin(), m_times.end(), std::back_inserter(latencies), [&](auto &result) {
return std::chrono::duration_cast<std::chrono::microseconds>(result.second - result.first);
});
std::future<double> avg = std::async(std::launch::async, [&latencies]() {
uint64_t total = 0;
std::for_each(latencies.begin(), latencies.end(), [&total](auto &v) {
total += v.count();
});
return static_cast<double>(total) / latencies.size();
});
std::future<std::chrono::microseconds> min = std::async(std::launch::async, [&latencies]() {
return *std::min_element(latencies.begin(), latencies.end());
});
std::future<std::chrono::microseconds> max = std::async(std::launch::async, [&latencies]() {
return *std::max_element(latencies.begin(), latencies.end());
});
auto find_p = [&](size_t p) {
size_t index = std::floor((static_cast<double>(100 - p) / 100.0) * latencies.size());
std::nth_element(latencies.begin(), latencies.begin() + index, latencies.end(), std::greater<std::chrono::microseconds>());
return latencies[index];
};
std::future<std::chrono::microseconds> p99 = std::async(std::launch::async, std::bind(find_p, 95));
std::future<std::chrono::microseconds> p90 = std::async(std::launch::async, std::bind(find_p, 90));
std::future<std::chrono::microseconds> p50 = std::async(std::launch::async, std::bind(find_p, 50));
INFO_LOG("Avg round trip: " << avg.get() << " us");
INFO_LOG("Min round trip: " << min.get().count() << " us");
INFO_LOG("Max round trip: " << max.get().count() << " us");
INFO_LOG("P99 round trip: " << p99.get().count() << " us");
INFO_LOG("P90 round trip: " << p90.get().count() << " us");
INFO_LOG("P50 round trip: " << p50.get().count() << " us");
} catch (const std::exception &stde) {
ERROR_LOG("Internal error: " << stde.what());
return 1;
}
}
ModifyActivityForm::ModifyActivityForm(QWidget* parent, int id, int activityGroupId): QDialog(parent)
{
setupUi(this);
durList.clear();
durList.append(duration1SpinBox);
durList.append(duration2SpinBox);
durList.append(duration3SpinBox);
durList.append(duration4SpinBox);
durList.append(duration5SpinBox);
durList.append(duration6SpinBox);
durList.append(duration7SpinBox);
durList.append(duration8SpinBox);
durList.append(duration9SpinBox);
durList.append(duration10SpinBox);
durList.append(duration11SpinBox);
durList.append(duration12SpinBox);
durList.append(duration13SpinBox);
durList.append(duration14SpinBox);
durList.append(duration15SpinBox);
durList.append(duration16SpinBox);
durList.append(duration17SpinBox);
durList.append(duration18SpinBox);
durList.append(duration19SpinBox);
durList.append(duration20SpinBox);
durList.append(duration21SpinBox);
durList.append(duration22SpinBox);
durList.append(duration23SpinBox);
durList.append(duration24SpinBox);
durList.append(duration25SpinBox);
durList.append(duration26SpinBox);
durList.append(duration27SpinBox);
durList.append(duration28SpinBox);
durList.append(duration29SpinBox);
durList.append(duration30SpinBox);
durList.append(duration31SpinBox);
durList.append(duration32SpinBox);
durList.append(duration33SpinBox);
durList.append(duration34SpinBox);
durList.append(duration35SpinBox);
for(int i=0; i<MAX_SPLIT_OF_AN_ACTIVITY; i++)
dur(i)->setMaximum(gt.rules.nHoursPerDay);
activList.clear();
activList.append(active1CheckBox);
activList.append(active2CheckBox);
activList.append(active3CheckBox);
activList.append(active4CheckBox);
activList.append(active5CheckBox);
activList.append(active6CheckBox);
activList.append(active7CheckBox);
activList.append(active8CheckBox);
activList.append(active9CheckBox);
activList.append(active10CheckBox);
activList.append(active11CheckBox);
activList.append(active12CheckBox);
activList.append(active13CheckBox);
activList.append(active14CheckBox);
activList.append(active15CheckBox);
activList.append(active16CheckBox);
activList.append(active17CheckBox);
activList.append(active18CheckBox);
activList.append(active19CheckBox);
activList.append(active20CheckBox);
activList.append(active21CheckBox);
activList.append(active22CheckBox);
activList.append(active23CheckBox);
activList.append(active24CheckBox);
activList.append(active25CheckBox);
activList.append(active26CheckBox);
activList.append(active27CheckBox);
activList.append(active28CheckBox);
activList.append(active29CheckBox);
activList.append(active30CheckBox);
activList.append(active31CheckBox);
activList.append(active32CheckBox);
activList.append(active33CheckBox);
activList.append(active34CheckBox);
activList.append(active35CheckBox);
allTeachersListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
selectedTeachersListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
allStudentsListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
selectedStudentsListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
allActivityTagsListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
selectedActivityTagsListWidget->setSelectionMode(QAbstractItemView::SingleSelection);
connect(cancelPushButton, SIGNAL(clicked()), this, SLOT(cancel()));
connect(okPushButton, SIGNAL(clicked()), this, SLOT(ok()));
connect(clearTeacherPushButton, SIGNAL(clicked()), this, SLOT(clearTeachers()));
connect(clearStudentsPushButton, SIGNAL(clicked()), this, SLOT(clearStudents()));
connect(allTeachersListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(addTeacher()));
connect(selectedTeachersListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(removeTeacher()));
connect(allStudentsListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(addStudents()));
connect(selectedStudentsListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(removeStudents()));
connect(clearActivityTagPushButton, SIGNAL(clicked()), this, SLOT(clearActivityTags()));
connect(allActivityTagsListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(addActivityTag()));
connect(selectedActivityTagsListWidget, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, SLOT(removeActivityTag()));
connect(showYearsCheckBox, SIGNAL(toggled(bool)), this, SLOT(showYearsChanged()));
connect(showGroupsCheckBox, SIGNAL(toggled(bool)), this, SLOT(showGroupsChanged()));
connect(showSubgroupsCheckBox, SIGNAL(toggled(bool)), this, SLOT(showSubgroupsChanged()));
connect(helpPushButton, SIGNAL(clicked()), this, SLOT(help()));
centerWidgetOnScreen(this);
restoreFETDialogGeometry(this);
QSize tmp3=subjectsComboBox->minimumSizeHint();
Q_UNUSED(tmp3);
this->_id=id;
this->_activityGroupId=activityGroupId;
for(int i=0; i<gt.rules.activitiesList.size(); i++){
Activity* act=gt.rules.activitiesList[i];
if(act->activityGroupId==this->_activityGroupId && act->id==this->_id)
this->_activity=act;
}
this->_teachers=this->_activity->teachersNames;
this->_subject = this->_activity->subjectName;
this->_activityTags = this->_activity->activityTagsNames;
this->_students=this->_activity->studentsNames;
int nSplit;
if(this->_activityGroupId!=0){
nSplit=0;
for(int i=0; i<gt.rules.activitiesList.size(); i++){
Activity* act=gt.rules.activitiesList[i];
if(act->activityGroupId==this->_activityGroupId){
if(nSplit>=MAX_SPLIT_OF_AN_ACTIVITY){
assert(0);
}
else{
if(this->_id==act->id)
subactivitiesTabWidget->setCurrentIndex(nSplit);
dur(nSplit)->setValue(act->duration);
activ(nSplit)->setChecked(act->active);
nSplit++;
}
}
}
}
else{
nSplit=1;
dur(0)->setValue(this->_activity->duration);
activ(0)->setChecked(this->_activity->active);
subactivitiesTabWidget->setCurrentIndex(0);
}
splitSpinBox->setMinimum(nSplit);
splitSpinBox->setMaximum(nSplit);
splitSpinBox->setValue(nSplit);
nStudentsSpinBox->setMinimum(-1);
nStudentsSpinBox->setMaximum(MAX_ROOM_CAPACITY);
nStudentsSpinBox->setValue(-1);
if(this->_activity->computeNTotalStudents==false)
nStudentsSpinBox->setValue(this->_activity->nTotalStudents);
updateStudentsListWidget();
updateTeachersListWidget();
updateSubjectsComboBox();
updateActivityTagsListWidget();
selectedStudentsListWidget->clear();
for(QStringList::Iterator it=this->_students.begin(); it!=this->_students.end(); it++)
selectedStudentsListWidget->addItem(*it);
for(int i=0; i<MAX_SPLIT_OF_AN_ACTIVITY; i++)
if(i<nSplit)
subactivitiesTabWidget->setTabEnabled(i, true);
else
subactivitiesTabWidget->setTabEnabled(i, false);
okPushButton->setDefault(true);
okPushButton->setFocus();
foreach(Teacher* tch, gt.rules.teachersList)
teacherNamesSet.insert(tch->name);
foreach(Subject* sbj, gt.rules.subjectsList)
subjectNamesSet.insert(sbj->name);
foreach(ActivityTag* at, gt.rules.activityTagsList)
activityTagNamesSet.insert(at->name);
/*foreach(StudentsYear* year, gt.rules.yearsList){
numberOfStudentsHash.insert(year->name, year->numberOfStudents);
foreach(StudentsGroup* group, year->groupsList){
numberOfStudentsHash.insert(group->name, group->numberOfStudents);
foreach(StudentsSubgroup* subgroup, group->subgroupsList){
numberOfStudentsHash.insert(subgroup->name, subgroup->numberOfStudents);
}
}
}*/
}
void ModifyActivityForm::ok()
{
//teachers
QStringList teachers_names;
if(selectedTeachersListWidget->count()<=0){
int t=QMessageBox::question(this, tr("FET question"),
tr("Do you really want to have the activity without teacher(s)?"),
QMessageBox::Yes|QMessageBox::No, QMessageBox::Yes);
if(t==QMessageBox::No)
return;
}
else{
for(int i=0; i<selectedTeachersListWidget->count(); i++){
//assert(gt.rules.searchTeacher(selectedTeachersListWidget->item(i)->text())>=0);
assert(teacherNamesSet.contains(selectedTeachersListWidget->item(i)->text()));
teachers_names.append(selectedTeachersListWidget->item(i)->text());
}
}
//subject
QString subject_name=subjectsComboBox->currentText();
/*int subject_index=gt.rules.searchSubject(subject_name);
if(subject_index<0){*/
bool found=subjectNamesSet.contains(subject_name);
if(!found){
QMessageBox::warning(this, tr("FET information"),
tr("Invalid subject"));
return;
}
//activity tag
QStringList activity_tags_names;
for(int i=0; i<selectedActivityTagsListWidget->count(); i++){
//assert(gt.rules.searchActivityTag(selectedActivityTagsListWidget->item(i)->text())>=0);
assert(activityTagNamesSet.contains(selectedActivityTagsListWidget->item(i)->text()));
activity_tags_names.append(selectedActivityTagsListWidget->item(i)->text());
}
//students
int numberOfStudents=0;
QStringList students_names;
if(selectedStudentsListWidget->count()<=0){
int t=QMessageBox::question(this, tr("FET question"),
tr("Do you really want to have the activity without student set(s)?"),
QMessageBox::Yes|QMessageBox::No, QMessageBox::Yes);
if(t==QMessageBox::No)
return;
}
else{
for(int i=0; i<selectedStudentsListWidget->count(); i++){
//assert(gt.rules.searchStudentsSet(selectedStudentsListWidget->item(i)->text())!=NULL);
/*assert(numberOfStudentsHash.contains(selectedStudentsListWidget->item(i)->text()));
numberOfStudents+=numberOfStudentsHash.value(selectedStudentsListWidget->item(i)->text());*/
assert(gt.rules.permanentStudentsHash.contains(selectedStudentsListWidget->item(i)->text()));
numberOfStudents+=gt.rules.permanentStudentsHash.value(selectedStudentsListWidget->item(i)->text())->numberOfStudents;
students_names.append(selectedStudentsListWidget->item(i)->text());
}
}
/*int total_number_of_students=0;
for(QStringList::Iterator it=students_names.begin(); it!=students_names.end(); it++){
StudentsSet* ss=gt.rules.searchStudentsSet(*it);
assert(ss!=NULL);
total_number_of_students+=ss->numberOfStudents;
}*/
int total_number_of_students=numberOfStudents;
int totalduration;
int durations[MAX_SPLIT_OF_AN_ACTIVITY];
bool active[MAX_SPLIT_OF_AN_ACTIVITY];
int nsplit=splitSpinBox->value();
totalduration=0;
for(int i=0; i<nsplit; i++){
durations[i]=dur(i)->value();
active[i]=activ(i)->isChecked();
totalduration+=durations[i];
}
if(nStudentsSpinBox->value()==-1){
gt.rules.modifyActivity(this->_id, this->_activityGroupId, teachers_names, subject_name,
activity_tags_names,students_names, nsplit, totalduration, durations, active,
(nStudentsSpinBox->value()==-1), total_number_of_students);
}
else{
gt.rules.modifyActivity(this->_id, this->_activityGroupId, teachers_names, subject_name,
activity_tags_names,students_names, nsplit, totalduration, durations, active,
(nStudentsSpinBox->value()==-1), nStudentsSpinBox->value());
}
PlanningChanged::increasePlanningCommunicationSpinBox();
this->accept();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "roomba560_light_node");
ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
double last_x, last_y, last_yaw;
double vel_x, vel_y, vel_yaw;
double dt;
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
private_nh.param<double>("test_vel", test_vel, 0.1);
private_nh.param<double>("test_odom_th", test_odom_th, 0.02);
test_vel_inc = test_vel / 20;
roomba = new irobot::OpenInterface(port.c_str());
ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 50);
sensor_msgs::JointState js;
js.name.resize(2);
js.position.resize(2);
js.velocity.resize(2);
js.effort.resize(2);
js.name[0] = "left_wheel_joint";
js.name[1] = "right_wheel_joint";
js.position[0] = 0.0;
js.position[1] = 0.0;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 5);
//tf::TransformBroadcaster odom_broadcaster;
ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdVelReceived);
if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
else
{
ROS_FATAL("Could not connect to Roomba.");
ROS_BREAK();
}
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
int heartvalue = 0;
bool inc = true;
bool packets_received = false;
unsigned int packets_not_received = 0;
unsigned int test_cnt = 0;
double test_vel_tmp = 0.0;
ros::Rate r(20.0);
while(n.ok())
{
current_time = ros::Time::now();
last_x = roomba->odometry_x_;
last_y = roomba->odometry_y_;
last_yaw = roomba->odometry_yaw_;
if (!command_tested) {
if (test_vel_tmp<test_vel) test_vel_tmp += test_vel_inc;
roomba->drive(test_vel_tmp,0.0);
roomba->setLeds(0, 0, 0, 0, 255, 255);
if (roomba->odometry_x_ > test_odom_th) {
roomba->drive(0.0,0.0);
command_tested = true;
ROS_INFO("Test of base driver was successful.");
} else {
if (test_cnt++ >= 20) { // 2s
roomba->powerDown();
roomba->closeSerialPort();
roomba_is_down = true;
ROS_ERROR("Test of base driver failed. Shutting down. Please press red button.");
ros::Duration dur(10);
dur.sleep();
break;
}
}
} else {
if(inc==true) heartvalue += 20;
else heartvalue -= 20;
if(heartvalue>=255) {
heartvalue = 255;
inc=false;
}
if(heartvalue<=0) {
heartvalue = 0;
inc=true;
}
roomba->setLeds(0, 0, 0, 0, (unsigned char)heartvalue, 255);
}
if( (roomba->getSensorPackets(100) == -1) && (packets_not_received++ >= 5) ) {
roomba->powerDown();
roomba->closeSerialPort();
roomba_is_down = true;
ROS_ERROR("Could not retrieve sensor packets. Shutting down. Please reset robot.");
ros::Duration dur(20);
dur.sleep();
break;
} else {
if (!packets_received) {
ROS_INFO("We are receiving sensor packets!");
packets_received = true;
}
packets_not_received = 0;
roomba->calculateOdometry();
}
dt = (current_time - last_time).toSec();
if (command_tested) roomba->drive(g_lin, g_ang);
vel_x = (roomba->odometry_x_ - last_x)/dt;
vel_y = (roomba->odometry_y_ - last_y)/dt;
vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
//first, we'll publish the transform over tf
/*geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = roomba->odometry_x_;
odom_trans.transform.translation.y = roomba->odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;*/
//send the transform
//odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
//odom.header.frame_id = "odom";
odom.header.frame_id = "odom_combined";
//set the position
odom.pose.pose.position.x = roomba->odometry_x_;
odom.pose.pose.position.y = roomba->odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//set the velocity
//odom.child_frame_id = "base_link";
odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
// ZdenekM -> added covariance, needed for robot_pose_ekf...
odom.pose.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
(0) (1e-3) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (1e3) ;
odom.twist.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
(0) (1e-3) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (1e3) ;
//publish the message
if (command_tested) {
odom_pub.publish(odom);
// stop if there is no command for more than 0.5s
if ((ros::Time::now() - latest_command) > ros::Duration(0.5)) {
g_ang = 0.0;
g_lin = 0.0;
roomba->drive(0.0,0.0);
}
}
js_pub.publish(js);
ros::spinOnce();
r.sleep();
}
roomba->setLeds(0, 0, 0, 0, 0, 0);
if (!roomba_is_down) {
roomba->powerDown();
roomba->closeSerialPort();
}
}
AddSyn& freq(float v1, float v2) {
dur(v1);
freq(v2);
return *this;
}
