KUKADU_SHARED_PTR<kukadu_thread> ControlQueue::startQueue() {
setInitValues();
thr = KUKADU_SHARED_PTR<kukadu_thread>(new kukadu_thread(&ControlQueue::run, this));
while(!this->isInitialized());
startQueueHook();
return thr;
}
/*!
* \brief Main routine for updating all components.
*/
void TeleopCOB::update()
{
if (!joy_active_)
{
if (!stopped_)
{
// stop components: send zero for one time
for(std::map<std::string,joint_module>::iterator module_it=joint_modules_.begin();module_it!=joint_modules_.end();++module_it)
{
for(unsigned int i=0; i<module_it->second.req_joint_vel_.size();i++)
{
module_it->second.req_joint_vel_[i] = 0.0;
}
}
if(has_base_module_)
{
for(unsigned int i=0; i<3; i++){
base_module_.req_vel_[i]=0;
base_module_.vel_old_[i]=0;
}
}
update_joint_modules();
update_base();
stopped_ = true;
ROS_INFO("stopped all components");
}
return;
}
// set initial values
if(!got_init_values_)
{
if (time_for_init_ < 1.0) // wait for 1 sec, then set init values to 0.0
{
ROS_DEBUG("still waiting for initial values, time_for_init_ = %f",time_for_init_);
time_for_init_ = time_for_init_ + 1.0/PUBLISH_FREQ;
return;
}
else
{
ROS_WARN("Timeout waiting for /joint_states message. Setting all init values to 0.0");
setInitValues();
}
}
update_joint_modules();
update_base();
stopped_ = false;
}
void ControlQueue::setInitValuesInternal() {
isInit = false;
finish = 0;
currentTime = 0.0;
rollbackMode = false;
rollBackQueueSize = 0;
currentJoints = arma::vec(1);
while(!movementQueue.empty()) movementQueue.pop();
while(!cartesianMovementQueue.empty()) cartesianMovementQueue.pop();
setInitValues();
}
void handle_init(void) {
my_window = window_create();
text_layer = text_layer_create(GRect(0, 0, 144, 60));
Layer *layer = window_get_root_layer(my_window);
layer_add_child(layer, text_layer_get_layer(text_layer));
text_layer_set_font(text_layer, fonts_get_system_font(FONT_KEY_GOTHIC_14));
text_layer_set_background_color(text_layer, GColorClear);
layer_set_update_proc(layer, layer_on_update);
window_stack_push(my_window, true);
setInitValues();
accel_data_service_subscribe(1, data_handler);
light_enable(true);
}
void ReachableList::calculateNewList()
{
// qDebug( "ReachableList::calculateNewList() is called" );
// calculateNewList is also called from calculator, so on check has to be
// executed
if ( !isOn() )
{
return;
}
// QTime t; // timer for performance measurement
// t.start();
setInitValues();
clearLists(); // clear all lists
// Now add items of different type to the list
addItemsToList(MapContents::AirfieldList);
addItemsToList(MapContents::GliderfieldList);
addItemsToList(MapContents::OutLandingList);
addItemsToList(MapContents::WaypointList);
modeAltitude = false;
//qDebug("Number of potential reachable sites: %d", count() );
// sort list according to distances
qSort(begin(), end() );
removeDoubles();
// qDebug("Number of potential reachable sites (after pruning): %d", count() );
// Remove all elements over the maximum. That are the far away elements.
int nr = getMaxNrOfSites();
while ( nr < size() )
{
removeFirst();
}
// qDebug("Limited Number of potential reachable sites: %d", count() );
calculateDataInList();
//qDebug("Time for full calculation: %d msec", t.restart() );
}
/*!
* \brief Executes the callback from the joint_states topic. (published by joint state driver)
*
* Only used to get the initaial joint positions.
*
* \param msg JointState
*/
void TeleopCOB::joint_states_cb(const sensor_msgs::JointState::ConstPtr &joint_states_msg)
{
if (!got_init_values_ && stopped_ && joy_active_)
{
ROS_DEBUG("joint_states_cb: getting init values");
for (unsigned int j = 0; j<joint_names_.size(); j++ )
{
for (unsigned int i = 0; i<joint_states_msg->name.size(); i++ )
{
ROS_DEBUG("joint names in init: %s should match %s",joint_names_[j].c_str(),joint_states_msg->name[i].c_str());
if (joint_states_msg->name[i] == joint_names_[j])
{
joint_init_values_[j] = joint_states_msg->position[i];
if(joint_names_[j]!=combined_joints_.joint_names_[j])
ROS_ERROR("error in new joint name collection, name miss match.");
combined_joints_.joint_init_values_[j] = joint_states_msg->position[i]; //new
ROS_DEBUG("joint %s found. init value = %f",joint_names_[j].c_str(),joint_init_values_[j]);
break;
}
}
}
setInitValues();
}
}
void KukieControlQueue::constructQueue(double sleepTime, std::string commandTopic, std::string retPosTopic, std::string switchModeTopic, std::string retCartPosTopic,
std::string cartStiffnessTopic, std::string jntStiffnessTopic, std::string ptpTopic,
std::string commandStateTopic, std::string ptpReachedTopic, std::string addLoadTopic, std::string jntFrcTrqTopic, std::string cartFrcTrqTopic,
std::string cartPtpTopic, std::string cartPtpReachedTopic, std::string cartMoveRfQueueTopic, std::string cartMoveWfQueueTopic, std::string cartPoseRfTopic, std::string jntSetPtpThreshTopic, bool acceptCollisions, ros::NodeHandle node
) {
set_ctrlc_exit_handler();
this->acceptCollisions = acceptCollisions;
this->ptpTopic = ptpTopic;
this->addLoadTopic = addLoadTopic;
this->commandTopic = commandTopic;
this->cartPtpTopic = cartPtpTopic;
this->retJointPosTopic = retPosTopic;
this->jntFrcTrqTopic = jntFrcTrqTopic;
this->switchModeTopic = switchModeTopic;
this->retCartPosTopic = retCartPosTopic;
this->ptpReachedTopic = ptpReachedTopic;
this->cartFrcTrqTopic = cartFrcTrqTopic;
this->stiffnessTopic = cartStiffnessTopic;
this->jntStiffnessTopic = jntStiffnessTopic;
this->commandStateTopic = commandStateTopic;
this->cartPtpReachedTopic = cartPtpReachedTopic;
this->cartMoveRfQueueTopic = cartMoveRfQueueTopic;
this->cartMoveWfQueueTopic = cartMoveWfQueueTopic;
this->jntSetPtpThreshTopic = jntSetPtpThreshTopic;
loop_rate = new ros::Rate(1.0 / sleepTime);
cartesianPtpReached = 0;
setInitValues();
this->node = node;
if(sleepTime == 0.0) {
ROS_ERROR("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
throw KukaduException("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
}
subJntPos = node.subscribe(retPosTopic, 2, &KukieControlQueue::robotJointPosCallback, this);
subComState = node.subscribe(commandStateTopic, 2, &KukieControlQueue::commandStateCallback, this);
subPtpReached = node.subscribe(ptpReachedTopic, 2, &KukieControlQueue::ptpReachedCallback, this);
subjntFrcTrq = node.subscribe(jntFrcTrqTopic, 2, &KukieControlQueue::jntFrcTrqCallback, this);
subCartFrqTrq = node.subscribe(cartFrcTrqTopic, 2, &KukieControlQueue::cartFrcTrqCallback, this);
subCartPtpReached = node.subscribe(cartPtpReachedTopic, 2, &KukieControlQueue::cartPtpReachedCallback, this);
subCartPoseRf = node.subscribe(cartPoseRfTopic, 2, &KukieControlQueue::cartPosRfCallback, this);
pub_set_cart_stiffness = node.advertise<iis_robot_dep::CartesianImpedance>(stiffnessTopic, 1);
pub_set_joint_stiffness = node.advertise<iis_robot_dep::FriJointImpedance>(jntStiffnessTopic, 1);
pubCartPtp = node.advertise<geometry_msgs::Pose>(cartPtpTopic, 1);
pubCartMoveRfQueue = node.advertise<geometry_msgs::Pose>(cartMoveRfQueueTopic, 1);
pubCartMoveWfQueue = node.advertise<geometry_msgs::Pose>(cartMoveWfQueueTopic, 1);
pub_set_ptp_thresh = node.advertise<std_msgs::Float64>(jntSetPtpThreshTopic, 1);
pubCommand = node.advertise<std_msgs::Float64MultiArray>(commandTopic, 10);
pubSwitchMode = node.advertise<std_msgs::Int32>(switchModeTopic, 1);
pubPtp = node.advertise<std_msgs::Float64MultiArray>(ptpTopic, 10);
isRealRobot = (getRobotDeviceType().compare("real")) ? false : true;
// this is required because shared_from_this can't be called in constructor (initializiation happens by lazy loading)
plannerInitialized = false;
kinematicsInitialized = false;
ros::Rate r(10);
while(!firstJointsReceived || !firstModeReceived)
r.sleep();
currentControlType = impMode;
currentCartFrqTrq = vec(6);
currentCartFrqTrq.fill(0.0);
currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
currentJntFrqTrq.fill(0.0);
usleep(1e6);
}
/**
* Calculate course, distance and reachability from the current position
* to the elements contained in the limited list. If a glider is defined
* the glide path is taken into account and the arrival altitude is
* calculated too.
*/
void ReachableList::calculateDataInList()
{
// QTime t;
// t.start();
int counter = 0;
setInitValues();
arrivalAltMap.clear();
distanceMap.clear();
for (int i = 0; i < count(); i++)
{
// recalculate Distance
ReachablePoint& p = (*this)[i];
WGSPoint pt = p.getWaypoint()->wgsPoint;
Altitude arrivalAlt;
Distance distance;
Speed bestSpeed;
distance.setKilometers( MapCalc::dist(&lastPosition, &pt) );
if ( lastPosition == pt || distance.getMeters() <= 100.0 )
{
// @AP: there is nearly no difference between the two points,
// therefore we have no distance and no bearing
distance.setMeters(0.0);
p.setDistance( distance );
p.setBearing( 0 );
p.setArrivalAlt( calculator->getAltitudeCollection().gpsAltitude );
}
else
{
p.setDistance( distance );
// recalculate Bearing
p.setBearing( short (rint(MapCalc::getBearingWgs(lastPosition, pt) * 180/M_PI)) );
// Calculate glide path. Returns false, if no glider is known.
calculator->glidePath( p.getBearing(), p.getDistance(),
Altitude(p.getElevation()),
arrivalAlt, bestSpeed );
// Save arrival altitude. Is set to invalid, if no glider is defined in calculator.
p.setArrivalAlt( arrivalAlt );
}
if ( arrivalAlt.isValid() )
{
// add only valid altitudes to the map
arrivalAltMap[ coordinateString ( pt ) ] = (int) arrivalAlt.getMeters() + safetyAlt;
}
distanceMap[ coordinateString ( pt ) ] = distance;
if ( arrivalAlt.getMeters() > 0 )
{
counter++;
}
}
// sorting of items depends on the glider selection
if ( calculator->glider() )
{
modeAltitude = true; // glider is known, sort by arrival altitudes
}
else
{
modeAltitude = false; // glider is unknown, sort by distances
}
qSort( begin(), end() );
// qDebug("Number of reachable sites (arriv >0): %d", counter );
// qDebug("Time for glide path calculation: %d msec", t.restart() );
emit newReachList();
}
void list_setup(Dynamic_File_List* p_DFL, list_attribs* p_LA, std::stack<Operation> * undo_stack, std::stack<Operation> * redo_stack){
size_t num_elems = p_LA->file_paths.size(); //number of files for this call
size_t sub_string_index;
std::vector<std::string> file_listing;
file_listing.clear();
for(int i = 0; i < num_elems; ++i){
sub_string_index = p_LA->file_paths[i].rfind(slash);
if(sub_string_index!=std::string::npos){
std::string tmp = p_LA->file_paths[i].substr(sub_string_index+1, ((int)p_LA->file_paths[i].size() - sub_string_index));
file_listing.push_back(tmp);
}
}
p_LA->full_path_prefix.clear();
p_LA->full_path_prefix = p_LA->file_paths[num_elems-1].substr(0, sub_string_index+1);
//instantiate matrix
mat_col_ind mat_i;
mat_i.clear();
resize_matrix(mat_i, file_listing.size(), 0);
char inp_delims[] = ".";
std::string tmp;
std::string tmp_ext;
for(int i = 0; i < file_listing.size(); i++){
token_arr token_file_fixs = token_arr_init(file_listing[i].c_str(), inp_delims);
while(*next_token( &token_file_fixs )){
mat_i[i].push_back(token_file_fixs.output);
}
if((int)mat_i[i].size() > 2){
for(int j = 0; j < ((int)mat_i[i].size() - 1); j++){
if(tmp=="") tmp = mat_i[i][j];
else tmp = tmp + "." + mat_i[i][j];
}
tmp_ext = mat_i[i][(int)mat_i[i].size()-1];
mat_i[i].clear();
mat_i[i].push_back(tmp);
mat_i[i].push_back(tmp_ext);
}
else if((int)mat_i[i].size() == 1){
mat_i[i].push_back("");
}
free_token_arr( &token_file_fixs );
}
//Populate DFL from file_listing matrix
//NOTE: This currently cannot handle there being 0 files chosen
row_node* p_newrow;
for(int i = 0; i < file_listing.size(); i++){
p_newrow = p_DFL->insertAtEnd(mat_i[i][0], i);
p_newrow->ext.push_back(mat_i[i][1]);
}
setInitValues(p_LA, p_DFL);
handler_loop(p_DFL, p_LA, undo_stack, redo_stack);
}
void PlottingControlQueue::construct(std::vector<std::string> jointNames, double timeStep) {
this->jointNames = jointNames;
setInitValues();
}
