void ModelPredictiveControlTrajectories::initializeParams()
{
ros::NodeHandle n_param ("~");
// **** get parameters
if (!n_param.getParam ("fmin", fmin))
fmin = 5.0;//[m/s**2]
ROS_INFO ("\t input constraint fmin: %d", fmin);
if (!n_param.getParam("fmax", fmax))
fmax = 50.0;//25//[m/s**2]
ROS_INFO("\t input constraint fmax: %f", fmax);
if (!n_param.getParam("wmax", wmax))
wmax = 40.0;//20//[rad/s]
ROS_INFO("\t input constraint wmax: %f", wmax);
if (!n_param.getParam("minTimeSec", minTimeSec))
minTimeSec = 0.02;//0.02[s]
ROS_INFO("\t input constraint minTimeSec: %f", minTimeSec);
if (!n_param.getParam("min_high_upon_base", min_high_upon_base_))
min_high_upon_base_ = 0.5;
ROS_INFO("\t min_high_upon_base: %f", min_high_upon_base_);
}
ARMultiPublisher::ARMultiPublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
{
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
std::string default_path = "data/object_4x4";
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
// **** get parameters
if (!n_param.getParam ("is_right_camera", isRightCamera_))
isRightCamera_ = false;
if (!n_param.getParam ("publish_tf", publishTf_))
publishTf_ = true;
ROS_INFO ("\tPublish transforms: %d", publishTf_);
if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
publishVisualMarkers_ = true;
ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
if (!n_param.getParam ("threshold", threshold_))
threshold_ = 100;
ROS_INFO ("\tThreshold: %d", threshold_);
//modifications to allow path list from outside the package
n_param.param ("marker_pattern_list", local_path, default_path);
if (local_path.compare(0,5,"data/") == 0){
//according to previous implementations, check if first 5 chars equal "data/"
sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
}
else{
//for new implementations, can pass a path outside the package_path
sprintf (pattern_filename_, "%s", local_path.c_str ());
}
ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
// **** subscribe
ROS_INFO ("Subscribing to info topic");
sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARMultiPublisher::camInfoCallback, this);
getCamInfo_ = false;
// **** advertse
arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
if(publishVisualMarkers_)
{
rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
}
}
ARPublisher::ARPublisher (ros::NodeHandle & n):n_ (n), configured_(false)
{
std::string path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
// **** get parameters
if (!n_param.getParam ("publish_tf", publishTf_))
publishTf_ = true;
ROS_INFO ("\tPublish transforms: %d", publishTf_);
if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
publishVisualMarkers_ = true;
ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
if (!n_param.getParam ("threshold", threshold_))
threshold_ = 100;
ROS_INFO ("\tThreshold: %d", threshold_);
if (!n_param.getParam ("marker_pattern_list", path)){
sprintf(pattern_filename_, "%s/data/objects_kinect", package_path.c_str());
}else{
sprintf(pattern_filename_, "%s", path.c_str());
}
ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
if (!n_param.getParam ("marker_data_directory", path)){
sprintf(data_directory_, "%s", package_path.c_str());
}else{
sprintf(data_directory_, "%s", path.c_str());
}
ROS_INFO ("Marker Data Directory: %s", data_directory_);
// **** subscribe
configured_ = false;
cloud_sub_ = n_.subscribe(cloudTopic_, 1, &ARPublisher::getTransformationCallback, this);
// **** advertise
arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_markers",0);
if(publishVisualMarkers_)
{
rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
}
}
ARDroneRos::ARDroneRos()
: image_transport(node_handle),
navdata_changed_(false),
video_changed_(false),
prev_time_(0.0),
prev_yaw_(0.0),
drift_correction_rate_(0.0),
correct_gyro_drift_(false)
{
cmd_vel_sub = node_handle.subscribe("/cmd_vel", 1, &ARDroneRos::cmdVelCallback, this);
takeoff_sub = node_handle.subscribe("/ardrone/takeoff", 1, &ARDroneRos::takeoffCallback, this);
reset_sub = node_handle.subscribe("/ardrone/reset", 1, &ARDroneRos::resetCallback, this);
land_sub = node_handle.subscribe("/ardrone/land", 1, &ARDroneRos::landCallback, this);
trim_sub = node_handle.subscribe("/ardrone/trim", 1, &ARDroneRos::trimCallback, this);
magCalib_sub = node_handle.subscribe("/ardrone/mag_calib", 1, &ARDroneRos::magCalibCallback, this);
gyroCalib_sub = node_handle.subscribe("/ardrone/gyro_calib", 1, &ARDroneRos::gyroCalibCallback, this);
toggleCam_service = node_handle.advertiseService("/ardrone/togglecam", &ARDroneRos::toggleCamCallback, this);
// check ROS parameters
ros::NodeHandle n_param ("~");
if(!n_param.getParam("compatibility_mode", compatibility_mode_))
compatibility_mode_ = true;
if(compatibility_mode_)
{
ROS_WARN("Compatibility mode is ON.");
navdata_pub = node_handle.advertise<ardrone2::Navdata>("/ardrone/navdata", 1);
}
else{
navdata_pub = node_handle.advertise<ardrone2::Navdata2>("/ardrone/navdata", 1);
}
image_pub = image_transport.advertiseCamera("/ardrone/image_raw", 1);
if(!n_param.getParam("resolution", requested_resolution_))
requested_resolution_ = 1280;
if(requested_resolution_ != 1280 && requested_resolution_ != 640)
{
ROS_WARN("Invalid resolution parameter, defauls to 1280x720px for ARDrone2.");
requested_resolution_ = 1280;
}
}
/**
* @fn void *startROS (void *user)
* @brief ROS thread.
*
* The main program wait until "ros_param_read" in order to allow the <br>
* ROS params to be also the Gtk Window and Widgets params.
* Then the ROS thread wait to the widgets creation before subcribing<br>
* to any topics, avoid to call public widget function for a widget not<br>
* yet created.
*/
void *startROS (void *user)
{
if (user != NULL)
{
struct arg *p_arg = (arg *) user;
ros::init (p_arg->argc, p_arg->argv, "gpsd_viewer");
ros::NodeHandle n;
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
ROS_INFO ("Starting GPSD Viewer");
// -----------------------------------------------------------------
// **** allow widget creation
data->ros_param_read = true;
// **** wait to widget creation
while (!data->widget_created)
{
ROS_DEBUG ("Waiting widgets creation");
}
// -----------------------------------------------------------------
// **** topics subscribing
ros::Subscriber fix_sub;
fix_sub = n.subscribe ("/fix", 1, &gpsFixCallback);
pub_cmd= n.advertise<gpsd_viewer::cmd>("cmd",5);
ROS_INFO ("Spinning");
ros::spin ();
}
// **** stop the gtk main loop
if (GTK_IS_WINDOW (data->window))
{
gtk_main_quit ();
}
pthread_exit (NULL);
}
/**
* @fn void *startROS (void *user)
* @brief ROS thread.
*
* The main program wait until "ros_param_read" in order to allow the <br>
* ROS params to be also the Gtk Window and Widgets params.
* Then the ROS thread wait to the widgets creation before subcribing<br>
* to any topics, avoid to call public widget function for a widget not<br>
* yet created.
*/
void *startROS (void *user)
{
if (user != NULL)
{
struct arg *p_arg = (arg *) user;
ros::init (p_arg->argc, p_arg->argv, "ground_station");
ros::NodeHandle n;
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
ROS_INFO ("Starting CityFlyer Ground Station");
// -----------------------------------------------------------------
// **** General window parameters
if (!n_param.getParam ("window_grayscale_color", data->grayscale_color))
data->grayscale_color = false;
ROS_DEBUG ("\tWindow grayscale color: %d", data->grayscale_color);
if (!n_param.getParam ("window_radial_color", data->radial_color))
data->radial_color = true;
ROS_DEBUG ("\tWindow radial color: %d", data->radial_color);
if (!n_param.getParam ("telemetry_refresh_rate", data->telemetry_refresh_rate))
data->telemetry_refresh_rate = 200;
ROS_DEBUG ("\tTelemetry refresh_rate: %d", data->telemetry_refresh_rate);
// -----------------------------------------------------------------
// **** Altimeter parameters
if (!n_param.getParam ("altimeter_unit_is_feet", data->altimeter_unit_is_feet))
data->altimeter_unit_is_feet = true;
ROS_DEBUG ("\tAltimeter unit is FEET: %d", data->altimeter_unit_is_feet);
if (!n_param.getParam ("altimeter_step_value", data->altimeter_step_value))
data->altimeter_step_value = 100;
ROS_DEBUG ("\tAltimeter step value: %d", data->altimeter_step_value);
// -----------------------------------------------------------------
// **** Variometer parameters
/**
* @todo check the variometer value computation
*/
data->variometer_unit_is_feet = data->altimeter_unit_is_feet;
ROS_DEBUG ("\tVariometer unit is FEET: %d", data->variometer_unit_is_feet);
if (!n_param.getParam ("variometer_step_value", data->variometer_step_value))
data->variometer_step_value = 100;
ROS_DEBUG ("\tVariometer step value: %d", data->variometer_step_value);
// -----------------------------------------------------------------
// **** Gauge1 parameters
std::string gauge1_name, gauge1_unit, gauge1_color_strip, gauge1_sub_step;
n_param.param ("gauge1_name", gauge1_name, std::string ("Gauge 1"));
n_param.param ("gauge1_unit", gauge1_unit, std::string ("(unit)"));
sprintf (data->gauge1_name_f, "<big>%s</big>\n<span foreground=\"orange\"><i>(%s)</i></span>", gauge1_name.c_str (),
gauge1_unit.c_str ());
ROS_DEBUG ("\tGauge 1 name : %s", data->gauge1_name_f);
if (!n_param.getParam ("gauge1_start_value", data->gauge1_start_value))
data->gauge1_start_value = 0;
ROS_DEBUG ("\tGauge 1 start value: %d", data->gauge1_start_value);
if (!n_param.getParam ("gauge1_end_value", data->gauge1_end_value))
data->gauge1_end_value = 100;
ROS_DEBUG ("\tGauge 1 end value: %d", data->gauge1_end_value);
if (!n_param.getParam ("gauge1_initial_step", data->gauge1_initial_step))
data->gauge1_initial_step = 10;
ROS_DEBUG ("\tGauge 1 initial step value: %d", data->gauge1_initial_step);
/*
* Can't figure out why I can't use a double in a ROS launch file
* To avoid wasting time, I use a string.
* (string)3_45 = (double)3.45
*/
if (!n_param.getParam ("gauge1_sub_step", gauge1_sub_step))
{
data->gauge1_sub_step = 2;
}
else
{
size_t found = gauge1_sub_step.find_first_of ("_");
gauge1_sub_step[found] = '.';
ROS_DEBUG ("\tGauge 1 sub step value: %s", gauge1_sub_step.c_str ());
try
{
data->gauge1_sub_step = boost::lexical_cast < double >(gauge1_sub_step);
}
catch (const std::exception &)
{
data->gauge1_sub_step = 0; // **** Will cause a Gtk warning on the gauge
}
}
if (!n_param.getParam ("gauge1_drawing_step", data->gauge1_drawing_step))
data->gauge1_drawing_step = 10;
ROS_DEBUG ("\tGauge 1 drawing step value: %d", data->gauge1_drawing_step);
// **** OPTIONAL
n_param.param ("gauge1_color_strip_order", gauge1_color_strip, std::string ("YOR"));
sprintf (data->gauge1_color_strip_order, "%s", gauge1_color_strip.c_str ());
n_param.getParam ("gauge1_green_strip_start", data->gauge1_green_strip_start);
n_param.getParam ("gauge1_yellow_strip_start", data->gauge1_yellow_strip_start);
n_param.getParam ("gauge1_red_strip_start", data->gauge1_red_strip_start);
// -----------------------------------------------------------------
// **** allow widget creation
data->ros_param_read = true;
// **** wait to widget creation
while (!data->widget_created)
{
ROS_DEBUG ("Waiting widgets creation");
}
// -----------------------------------------------------------------
// **** topics subscribing
ROS_INFO ("Subscribing to topics");
imuSub = n.subscribe (imuTopic, 1, imuCallback);
heightSub = n.subscribe (heightTopic, 1, heightCallback);
imuCalcDataSub = n.subscribe (imuCalcDataTopic, 1, imuCalcDataCallback);
//~ gpsDataSub = n.subscribe (gpsDataTopic, 1, gpsDataCallback);
llStatusSub = n.subscribe (llStatusTopic, 1, llStatusCallback);
gpsFixSub = n.subscribe ("/fix", 1, &gpsFixCallback);
ROS_INFO ("Spinning");
ros::spin ();
}
// **** stop the gtk main loop
if (GTK_IS_WIDGET (data->window))
{
gtk_main_quit ();
}
pthread_exit (NULL);
}
ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
{
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
ROS_INFO("Starting ArSinglePublisher");
// **** get parameters
if (!n_param.getParam("publish_tf", publishTf_))
publishTf_ = true;
ROS_INFO ("\tPublish transforms: %d", publishTf_);
if (!n_param.getParam("publish_visual_markers", publishVisualMarkers_))
publishVisualMarkers_ = true;
ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
if (!n_param.getParam("threshold", threshold_))
threshold_ = 100;
ROS_INFO ("\tThreshold: %d", threshold_);
if (!n_param.getParam("marker_width", markerWidth_))
markerWidth_ = 80.0;
ROS_INFO ("\tMarker Width: %.1f", markerWidth_);
if (!n_param.getParam("reverse_transform", reverse_transform_))
reverse_transform_ = false;
ROS_INFO("\tReverse Transform: %d", reverse_transform_);
if (!n_param.getParam("marker_frame", markerFrame_))
markerFrame_ = "ar_marker";
ROS_INFO ("\tMarker frame: %s", markerFrame_.c_str());
// If mode=0, we use arGetTransMat instead of arGetTransMatCont
// The arGetTransMatCont function uses information from the previous image
// frame to reduce the jittering of the marker
if (!n_param.getParam("use_history", useHistory_))
useHistory_ = true;
ROS_INFO("\tUse history: %d", useHistory_);
n_param.param ("marker_pattern", local_path, std::string ("data/patt.hiro"));
sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
ROS_INFO ("\tMarker Pattern Filename: %s", pattern_filename_);
n_param.param ("marker_center_x", marker_center_[0], 0.0);
n_param.param ("marker_center_y", marker_center_[1], 0.0);
ROS_INFO ("\tMarker Center: (%.1f,%.1f)", marker_center_[0], marker_center_[1]);
// **** subscribe
ROS_INFO ("Subscribing to info topic");
sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
getCamInfo_ = false;
// **** advertsie
arMarkerPub_ = n_.advertise<ar_pose::ARMarker>("ar_pose_marker", 0);
if(publishVisualMarkers_){
rvizMarkerPub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
}
}
void CompNovoIdentificationCID::getIdentification(PeptideIdentification & id, const PeakSpectrum & CID_spec)
{
//if (CID_spec.getPrecursors().begin()->getMZ() > 1000.0)
//{
//cerr << "Weight of precursor has been estimated to exceed 2000.0 Da which is the current limit" << endl;
//return;
//}
PeakSpectrum new_CID_spec(CID_spec);
windowMower_(new_CID_spec, 0.3, 1);
Param zhang_param;
zhang_param = zhang_.getParameters();
zhang_param.setValue("tolerance", fragment_mass_tolerance_);
zhang_param.setValue("use_gaussian_factor", "true");
zhang_param.setValue("use_linear_factor", "false");
zhang_.setParameters(zhang_param);
Normalizer normalizer;
Param n_param(normalizer.getParameters());
n_param.setValue("method", "to_one");
normalizer.setParameters(n_param);
normalizer.filterSpectrum(new_CID_spec);
Size charge(2);
double precursor_weight(0); // [M+H]+
if (!CID_spec.getPrecursors().empty())
{
// believe charge of spectrum?
if (CID_spec.getPrecursors().begin()->getCharge() != 0)
{
charge = CID_spec.getPrecursors().begin()->getCharge();
}
else
{
// TODO estimate charge state
}
precursor_weight = CID_spec.getPrecursors().begin()->getMZ() * charge - ((charge - 1) * Constants::PROTON_MASS_U);
}
//cerr << "charge=" << charge << ", [M+H]=" << precursor_weight << endl;
// now delete all peaks that are right of the estimated precursor weight
Size peak_counter(0);
for (PeakSpectrum::ConstIterator it = new_CID_spec.begin(); it != new_CID_spec.end(); ++it, ++peak_counter)
{
if (it->getPosition()[0] > precursor_weight)
{
break;
}
}
if (peak_counter < new_CID_spec.size())
{
new_CID_spec.resize(peak_counter);
}
static double oxonium_mass = EmpiricalFormula("H2O+").getMonoWeight();
Peak1D p;
p.setIntensity(1);
p.setPosition(oxonium_mass);
new_CID_spec.push_back(p);
p.setPosition(precursor_weight);
new_CID_spec.push_back(p);
// add complement to spectrum
/*
for (PeakSpectrum::ConstIterator it1 = CID_spec.begin(); it1 != CID_spec.end(); ++it1)
{
// get m/z of complement
double mz_comp = precursor_weight - it1->getPosition()[0] + Constants::PROTON_MASS_U;
// search if peaks are available that have similar m/z values
Size count(0);
bool found(false);
for (PeakSpectrum::ConstIterator it2 = CID_spec.begin(); it2 != CID_spec.end(); ++it2, ++count)
{
if (fabs(mz_comp - it2->getPosition()[0]) < fragment_mass_tolerance)
{
// add peak intensity to corresponding peak in new_CID_spec
new_CID_spec[count].setIntensity(new_CID_spec[count].getIntensity());
}
}
if (!found)
{
// infer this peak
Peak1D p;
p.setIntensity(it1->getIntensity());
p.setPosition(mz_comp);
new_CID_spec.push_back(p);
}
}*/
CompNovoIonScoringCID ion_scoring;
Param ion_scoring_param(ion_scoring.getParameters());
ion_scoring_param.setValue("fragment_mass_tolerance", fragment_mass_tolerance_);
ion_scoring_param.setValue("precursor_mass_tolerance", precursor_mass_tolerance_);
ion_scoring_param.setValue("decomp_weights_precision", decomp_weights_precision_);
ion_scoring_param.setValue("double_charged_iso_threshold", (double)param_.getValue("double_charged_iso_threshold"));
ion_scoring_param.setValue("max_isotope_to_score", param_.getValue("max_isotope_to_score"));
ion_scoring_param.setValue("max_isotope", max_isotope_);
ion_scoring.setParameters(ion_scoring_param);
Map<double, IonScore> ion_scores;
ion_scoring.scoreSpectrum(ion_scores, new_CID_spec, precursor_weight, charge);
new_CID_spec.sortByPosition();
/*
cerr << "Size of ion_scores " << ion_scores.size() << endl;
for (Map<double, IonScore>::const_iterator it = ion_scores.begin(); it != ion_scores.end(); ++it)
{
cerr << it->first << " " << it->second.score << endl;
}*/
#ifdef WRITE_SCORED_SPEC
PeakSpectrum filtered_spec(new_CID_spec);
filtered_spec.clear();
for (Map<double, CompNovoIonScoringCID::IonScore>::const_iterator it = ion_scores.begin(); it != ion_scores.end(); ++it)
{
Peak1D p;
p.setIntensity(it->second.score);
p.setPosition(it->first);
filtered_spec.push_back(p);
}
DTAFile().store("spec_scored.dta", filtered_spec);
#endif
set<String> sequences;
getDecompositionsDAC_(sequences, 0, new_CID_spec.size() - 1, precursor_weight, new_CID_spec, ion_scores);
#ifdef SPIKE_IN
sequences.insert("AFCVDGEGR");
sequences.insert("APEFAAPWPDFVPR");
sequences.insert("AVKQFEESQGR");
sequences.insert("CCTESLVNR");
sequences.insert("DAFLGSFLYEYSR");
sequences.insert("DAIPENLPPLTADFAEDK");
sequences.insert("DDNKVEDIWSFLSK");
sequences.insert("DDPHACYSTVFDK");
sequences.insert("DEYELLCLDGSR");
sequences.insert("DGAESYKELSVLLPNR");
sequences.insert("DGASCWCVDADGR");
sequences.insert("DLFIPTCLETGEFAR");
sequences.insert("DTHKSEIAHR");
sequences.insert("DVCKNYQEAK");
sequences.insert("EACFAVEGPK");
sequences.insert("ECCHGDLLECADDR");
sequences.insert("EFLGDKFYTVISSLK");
sequences.insert("EFTPVLQADFQK");
sequences.insert("ELFLDSGIFQPMLQGR");
sequences.insert("ETYGDMADCCEK");
sequences.insert("EVGCPSSSVQEMVSCLR");
sequences.insert("EYEATLEECCAK");
sequences.insert("FADLIQSGTFQLHLDSK");
sequences.insert("FFSASCVPGATIEQK");
sequences.insert("FLANVSTVLTSK");
sequences.insert("FLSGSDYAIR");
sequences.insert("FTASCPPSIK");
sequences.insert("GAIEWEGIESGSVEQAVAK");
sequences.insert("GDVAFIQHSTVEENTGGK");
sequences.insert("GEPPSCAEDQSCPSER");
sequences.insert("GEYVPTSLTAR");
sequences.insert("GQEFTITGQKR");
sequences.insert("GTFAALSELHCDK");
sequences.insert("HLVDEPQNLIK");
sequences.insert("HQDCLVTTLQTQPGAVR");
sequences.insert("HTTVNENAPDQK");
sequences.insert("ILDCGSPDTEVR");
sequences.insert("KCPSPCQLQAER");
sequences.insert("KGTEFTVNDLQGK");
sequences.insert("KQTALVELLK");
sequences.insert("KVPQVSTPTLVEVSR");
sequences.insert("LALQFTTNAKR");
sequences.insert("LCVLHEKTPVSEK");
sequences.insert("LFTFHADICTLPDTEK");
sequences.insert("LGEYGFQNALIVR");
sequences.insert("LHVDPENFK");
sequences.insert("LKECCDKPLLEK");
sequences.insert("LKHLVDEPQNLIK");
sequences.insert("LKPDPNTLCDEFK");
sequences.insert("LLGNVLVVVLAR");
sequences.insert("LLVVYPWTQR");
sequences.insert("LRVDPVNFK");
sequences.insert("LTDEELAFPPLSPSR");
sequences.insert("LVNELTEFAK");
sequences.insert("MFLSFPTTK");
sequences.insert("MPCTEDYLSLILNR");
sequences.insert("NAPYSGYSGAFHCLK");
sequences.insert("NECFLSHKDDSPDLPK");
sequences.insert("NEPNKVPACPGSCEEVK");
sequences.insert("NLQMDDFELLCTDGR");
sequences.insert("QAGVQAEPSPK");
sequences.insert("RAPEFAAPWPDFVPR");
sequences.insert("RHPEYAVSVLLR");
sequences.insert("RPCFSALTPDETYVPK");
sequences.insert("RSLLLAPEEGPVSQR");
sequences.insert("SAFPPEPLLCSVQR");
sequences.insert("SAGWNIPIGTLLHR");
sequences.insert("SCWCVDEAGQK");
sequences.insert("SGNPNYPHEFSR");
sequences.insert("SHCIAEVEK");
sequences.insert("SISSGFFECER");
sequences.insert("SKYLASASTMDHAR");
sequences.insert("SLHTLFGDELCK");
sequences.insert("SLLLAPEEGPVSQR");
sequences.insert("SPPQCSPDGAFRPVQCK");
sequences.insert("SREGDPLAVYLK");
sequences.insert("SRQIPQCPTSCER");
sequences.insert("TAGTPVSIPVCDDSSVK");
sequences.insert("TCVADESHAGCEK");
sequences.insert("TQFGCLEGFGR");
sequences.insert("TVMENFVAFVDK");
sequences.insert("TYFPHFDLSHGSAQVK");
sequences.insert("TYMLAFDVNDEK");
sequences.insert("VDEVGGEALGR");
sequences.insert("VDLLIGSSQDDGLINR");
sequences.insert("VEDIWSFLSK");
sequences.insert("VGGHAAEYGAEALER");
sequences.insert("VGTRCCTKPESER");
sequences.insert("VKVDEVGGEALGR");
sequences.insert("VKVDLLIGSSQDDGLINR");
sequences.insert("VLDSFSNGMK");
sequences.insert("VLSAADKGNVK");
sequences.insert("VPQVSTPTLVEVSR");
sequences.insert("VTKCCTESLVNR");
sequences.insert("VVAASDASQDALGCVK");
sequences.insert("VVAGVANALAHR");
sequences.insert("YICDNQDTISSK");
sequences.insert("YLASASTMDHAR");
sequences.insert("YNGVFQECCQAEDK");
#endif
SpectrumAlignmentScore spectra_zhang;
spectra_zhang.setParameters(zhang_param);
vector<PeptideHit> hits;
Size missed_cleavages = param_.getValue("missed_cleavages");
for (set<String>::const_iterator it = sequences.begin(); it != sequences.end(); ++it)
{
Size num_missed = countMissedCleavagesTryptic_(*it);
if (missed_cleavages < num_missed)
{
//cerr << "Two many missed cleavages: " << *it << ", found " << num_missed << ", allowed " << missed_cleavages << endl;
continue;
}
PeakSpectrum CID_sim_spec;
getCIDSpectrum_(CID_sim_spec, *it, charge);
//normalizer.filterSpectrum(CID_sim_spec);
double cid_score = zhang_(CID_sim_spec, CID_spec);
PeptideHit hit;
hit.setScore(cid_score);
hit.setSequence(getModifiedAASequence_(*it));
hit.setCharge((Int)charge); //TODO unify charge interface: int or size?
hits.push_back(hit);
//cerr << getModifiedAASequence_(*it) << " " << cid_score << " " << endl;
}
// rescore the top hits
id.setHits(hits);
id.assignRanks();
hits = id.getHits();
SpectrumAlignmentScore alignment_score;
Param align_param(alignment_score.getParameters());
align_param.setValue("tolerance", fragment_mass_tolerance_);
align_param.setValue("use_linear_factor", "true");
alignment_score.setParameters(align_param);
for (vector<PeptideHit>::iterator it = hits.begin(); it != hits.end(); ++it)
{
//cerr << "Pre: " << it->getRank() << " " << it->getSequence() << " " << it->getScore() << " " << endl;
}
Size number_of_prescoring_hits = param_.getValue("number_of_prescoring_hits");
if (hits.size() > number_of_prescoring_hits)
{
hits.resize(number_of_prescoring_hits);
}
for (vector<PeptideHit>::iterator it = hits.begin(); it != hits.end(); ++it)
{
PeakSpectrum CID_sim_spec;
getCIDSpectrum_(CID_sim_spec, getModifiedStringFromAASequence_(it->getSequence()), charge);
normalizer.filterSpectrum(CID_sim_spec);
//DTAFile().store("sim_specs/" + it->getSequence().toUnmodifiedString() + "_sim_CID.dta", CID_sim_spec);
//double cid_score = spectra_zhang(CID_sim_spec, CID_spec);
double cid_score = alignment_score(CID_sim_spec, CID_spec);
//cerr << "Final: " << it->getSequence() << " " << cid_score << endl;
it->setScore(cid_score);
}
id.setHits(hits);
id.assignRanks();
hits = id.getHits();
for (vector<PeptideHit>::iterator it = hits.begin(); it != hits.end(); ++it)
{
//cerr << "Fin: " << it->getRank() << " " << it->getSequence() << " " << it->getScore() << " " << endl;
}
Size number_of_hits = param_.getValue("number_of_hits");
if (id.getHits().size() > number_of_hits)
{
hits.resize(number_of_hits);
}
id.setHits(hits);
id.assignRanks();
return;
}
ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n, double x, double y, double z, double rx, double ry, double rz):n_ (n), it_ (n_)
{
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
std::string default_path = "data/patt.hiro";
ros::NodeHandle n_param ("~");
//btQuaternion R (rx,ry,rz);
//btVector3 T (x,y,z);
transform_.setOrigin(btVector3 (x,y,z));
transform_.setRotation(btQuaternion (rx,ry,rz));
XmlRpc::XmlRpcValue xml_marker_center;
ROS_INFO("Starting ArSinglePublisher");
// **** get parameters
if (!n_param.getParam("publish_tf", publishTf_))
publishTf_ = true;
ROS_INFO ("\tPublish transforms: %d", publishTf_);
if (!n_param.getParam("publish_visual_markers", publishVisualMarkers_))
publishVisualMarkers_ = true;
ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
if (!n_param.getParam("threshold", threshold_))
threshold_ = 100;
ROS_INFO ("\tThreshold: %d", threshold_);
if (!n_param.getParam("marker_width", markerWidth_))
markerWidth_ = 80.0;
ROS_INFO ("\tMarker Width: %.1f", markerWidth_);
if (!n_param.getParam("reverse_transform", reverse_transform_))
reverse_transform_ = false;
ROS_INFO("\tReverse Transform: %d", reverse_transform_);
if (!n_param.getParam("marker_frame", markerFrame_))
markerFrame_ = "ar_marker";
ROS_INFO ("\tMarker frame: %s", markerFrame_.c_str());
// If mode=0, we use arGetTransMat instead of arGetTransMatCont
// The arGetTransMatCont function uses information from the previous image
// frame to reduce the jittering of the marker
if (!n_param.getParam("use_history", useHistory_))
useHistory_ = true;
ROS_INFO("\tUse history: %d", useHistory_);
// n_param.param ("marker_pattern", local_path, std::string ("data/patt.hiro"));
// sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
// ROS_INFO ("\tMarker Pattern Filename: %s", pattern_filename_);
//modifications to allow patterns to be loaded from outside the package
n_param.param ("marker_pattern", local_path, default_path);
if (local_path.compare(0,5,"data/") == 0){
//to keep working on previous implementations, check if first 5 chars equal "data/"
sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
}
else{
//for new implementations, can pass a path outside the package_path
sprintf (pattern_filename_, "%s", local_path.c_str ());
}
n_param.param ("marker_center_x", marker_center_[0], 0.0);
n_param.param ("marker_center_y", marker_center_[1], 0.0);
ROS_INFO ("\tMarker Center: (%.1f,%.1f)", marker_center_[0], marker_center_[1]);
// **** subscribe
ROS_INFO ("Subscribing to info topic");
sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
getCamInfo_ = false;
;
// **** advertsie
arMarkerPub_ = n_.advertise<ar_pose::ARMarker>("ar_pose_marker", 0);
if(publishVisualMarkers_){
rvizMarkerPub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
}
}
ARBundlePublisher::ARBundlePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
{
std::string local_path;
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
std::string default_path = "data/object_4x4";
std::string default_path_tfs = "data/tfs";
std::string default_path_output = "data/pose_info.txt";
ros::NodeHandle n_param ("~");
XmlRpc::XmlRpcValue xml_marker_center;
// **** get parameters
if (!n_param.getParam ("publish_tf", publishTf_))
publishTf_ = true;
ROS_INFO ("\tPublish transforms: %d", publishTf_);
if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
publishVisualMarkers_ = true;
ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
if (!n_param.getParam ("threshold", threshold_))
threshold_ = 100;
ROS_INFO ("\tThreshold: %d", threshold_);
//modifications to allow path list from outside the package
n_param.param ("marker_pattern_list", local_path, default_path);
if (local_path.compare(0,5,"data/") == 0){
//according to previous implementations, check if first 5 chars equal "data/"
sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
}
else{
//for new implementations, can pass a path outside the package_path
sprintf (pattern_filename_, "%s", local_path.c_str ());
}
ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
//grab transform file name
n_param.param ("marker_transforms_list", local_path, default_path_tfs);
sprintf (transforms_filename_, "%s", local_path.c_str ());
ROS_INFO ("Transforms Filename: %s", transforms_filename_);
//grab output txt file name
if (!n_param.getParam ("output_pose", outputToFile)) {
outputToFile = false;
ROS_INFO ("\tNot outputting pose to file");
}
else {
if(outputToFile) {
ROS_INFO ("Outputting Pose");
n_param.param ("pose_output_file_prefix", local_path, default_path_output);
sprintf (pose_output_filename_, "%s", local_path.c_str ());
ROS_INFO ("pose output Filename prefix: %s", pose_output_filename_);
//open a timestamped one
memset(&buffer[0], '\0', sizeof(buffer));
time_t rawtime;
struct tm* timeinfo;
time(&rawtime);
timeinfo = localtime (&rawtime);
strftime (buffer,80,"-%F-%H-%M-%S.txt",timeinfo);
std::string str(buffer);
local_path.append(str);
output.open(local_path.c_str());
output << "#Pose estimation of object wrt camera. X forward, Y left, Z up\n#time in seconds since epoch,position(x), position(y), position(z), q_x, q_y, q_z, q_w \n" << std::endl;
memset(&buffer[0], '\0', sizeof(buffer));
}
else {
ROS_INFO ("\tNot outputting pose to file");
}
}
// **** subscribe
ROS_INFO ("Subscribing to info topic");
sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARBundlePublisher::camInfoCallback, this);
getCamInfo_ = false;
// **** advertse
arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
if(publishVisualMarkers_)
{
rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
}
}
