void TaurobClawNode::write(ros::Time time, ros::Duration period){
ROS_DEBUG_THROTTLE(1,"rotation_to_command: %f", joint_pos_cmd_rotation);
double rotation = rot_factor*joint_pos_cmd_rotation - rot_offset;
claw->Set_rotation(rotation);
ROS_DEBUG_THROTTLE(1,"rotation_commanded: %f", rotation);
ROS_DEBUG_THROTTLE(1,"gripper_to_command: %f", joint_pos_cmd_grip);
double grip = grip_factor*joint_pos_cmd_grip - grip_offset;
claw->Set_grip(grip);
ROS_DEBUG_THROTTLE(1,"gripper_commanded: %f", grip);
}
// verfiy input: does vector meet min/max jump step
bool validPoint(DataContainer * container, std::vector<double>& v_new, std::vector<double>& v_before)
{
ROS_WARN("new avg\n" );
fflush(stdout);
// no validation
if (container->getInt(13) == 0) {
return true;
}
// otherwise calculate distances:
// minimum distance to previous steering point, jump range = [min,max]
double min = 10;
// maximum ...
double max = 500;
// distance between new and last point
double distance =
sqrt((v_new[3] - v_before[3]) * (v_new[3] - v_before[3]) + (v_new[4] - v_before[4]) * (v_new[4] - v_before[4]) +
(v_new[5] - v_before[5]) * (v_new[5] - v_before[5]));
if (distance < min)
{
// endpoint arrived, no micro-adjustments below min
ROS_DEBUG_THROTTLE(1, NNAME ": below jump range %f < %f [mm]", distance, min);
return false;
}
else if (distance > max)
{
// distance exceeds max, thus is out of jump range
ROS_WARN_THROTTLE(1, NNAME ": exceeding jump range %f < %f [mm]", distance, max);
return false;
}
// otherwise all ok
return true;
}
void KinectDriver::processRgbAndDepth()
{
/// @todo Investigate how well synchronized the depth & color images are
// Fill raw RGB image message
if (pub_rgb_.getNumSubscribers () > 0 &&
config_.color_format != FREENECT_FORMAT_IR)
{
// Copy the image data
memcpy (&rgb_image_.data[0], &rgb_buf_[0], rgb_image_.data.size());
// Check the camera info
/// @todo Check this on loading info instead
if (rgb_info_.height != (size_t)rgb_image_.height || rgb_info_.width != (size_t)rgb_image_.width)
ROS_DEBUG_THROTTLE (60, "[KinectDriver::rgbCb] Uncalibrated Camera");
}
if (pub_ir_.getNumSubscribers () > 0 &&
config_.color_format == FREENECT_FORMAT_IR)
{
/// @todo Publish original 16-bit output? Shifting down to 8-bit for convenience for now
freenect_pixel_ir *ir = reinterpret_cast<freenect_pixel_ir*>(rgb_buf_);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
int val = ir[i];
rgb_image_.data[i] = (val >> 2) & 0xff;
}
}
void LeineLindeEncoder::processRXEvent(const fmMsgs::can::ConstPtr & msg)
{
if((uint16_t)msg->id == (LL_DEF_TPDO1 + this->node_id))
{
//return to upper layers XXX: perhaps use a queue here
tpdo_received = true;
tpdo_msg = *msg;
}
else if((uint16_t)msg->id == (LL_DEF_SDO_RX_RESPONSE + this->node_id))
{
if(sdo_reply_received)
{
ROS_ERROR("Unhandled SDO reply!");
}
else
{
// sdo reply received put in received queue
sdo_reply_received= true;
sdo_reply_msg = *msg;
}
}
else if((uint16_t)msg->id == (LL_DEF_HEARTBEAT + this->node_id))
{
ROS_DEBUG_THROTTLE(1,"HEARTBEAT Detected, %d",msg->data[0]);
// heart beat received store time at which it was received
last_heartbeat = ros::Time::now();
// store encoder state
encoder_state = (ll_canopen_nmt_state)(msg->data[0] & (0x7F));
}
}
void timercb(const ros::TimerEvent& e)
{
static int num_delays = 0;
ROS_DEBUG("coordinator timercb triggered");
if (gen_flag)
{
// Generate the robot ordering vector
gen_flag = generate_order();
return;
}
// get operating condition
if (ros::param::has("/operating_condition"))
ros::param::get("/operating_condition", operating_condition);
else
{
operating_condition = 4;
ros::param::set("/operating_condition", operating_condition);
}
// check to see if we are in run state
if(operating_condition == 1 || operating_condition == 2)
{
if(calibrated_flag)
{
if (num_delays < NUM_FRAME_DELAYS)
{
num_delays++;
return;
}
else if (num_delays < NUM_EKF_INITS+NUM_FRAME_DELAYS)
process_robots(1);
else
process_robots(operating_condition);
num_delays++;
}
return;
}
// are we in idle or stop condition?
else if(operating_condition == 0 || operating_condition == 3)
ROS_DEBUG_THROTTLE(1,"Coordinator node is idle due to operating condition");
// are we in emergency stop condition?
else if(operating_condition == 4)
ROS_WARN_THROTTLE(1,"Emergency Stop Requested");
// otherwise something terrible has happened
else
ROS_ERROR("Invalid value for operating_condition");
calibrated_flag = false;
calibrate_count = 0;
num_delays = 0;
return;
}
bool waitForMessage(T& msg, double timeout = -1)
{
if(!m_nh)
{
m_nh = new ros::NodeHandle("~");
}
if(!m_smt.initialized())
{
ROS_DEBUG_THROTTLE(1.0, "%s: Tried to get message from an uninitialized shared memory transport!", m_nh->getNamespace().c_str());
return false;
}
if(!m_smt.connected() && !m_smt.connect(timeout))
{
ROS_DEBUG_THROTTLE(1.0, "%s: Tried to get message from an unconnected shared memory transport and reconnection attempt failed!", m_nh->getNamespace().c_str());
return false;
}
if(!m_smt.awaitNewDataPolled(msg, timeout))
{
return false;
}
return true;
}
void
PersonTracker::computeStateLoop(const ros::TimerEvent& event) {
ROS_DEBUG("[person tracker] Last callback took %f seconds", event.profile.last_duration.toSec());
// Set variance based on time since last acquisition
double dt = timeSinceLastDetect().toSec();
double current_var = std::min(var_increase_rate_*dt, max_var_);
setVariance(person_pos_, current_var);
// person_pos_ is in the frame "map" which is constant. Lie and say the pose was acquired right now
person_pos_.header.stamp = ros::Time::now();
// Low-pass filter on the person position
PoseWithCovarianceStamped pub_pos;
pub_pos.header = person_pos_.header;
pub_pos.pose.covariance = person_pos_.pose.covariance;
pub_pos.pose.pose.orientation = person_pos_.pose.pose.orientation;
pub_pos.pose.pose.position.x = alpha_ *person_pos_ .pose.pose.position.x
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.x;
pub_pos.pose.pose.position.y = alpha_ *person_pos_ .pose.pose.position.y
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.y;
pub_pos.pose.pose.position.z = alpha_ *person_pos_ .pose.pose.position.z
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.z;
// Publish the goal and a stripped-down version without the covariance
goal_cov_pub_.publish( pub_pos );
goal_pub_ .publish( stripCovariance(pub_pos) );
last_pub_pos_ = pub_pos;
ROS_DEBUG_THROTTLE(2, "[person_tracker] Time since detect = %.2fs, Variance = %.2fm^2", dt, current_var);
// Update the sound player
if( hasPerson() ) {
sound_player_.setState(PTSounds::state_tracking);
}
else {
sound_player_.setState(PTSounds::state_searching);
}
sound_player_.update();
ros::spinOnce();
}
/**
* \brief Once started, this node periodically adapts the operator's transforms
*
* This node instantiates the motion_adaption class and
* periodically adapts the currently available operator transforms.
*
* @param argc not used
* @param argv not used
* @return always 0
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "motion_adaption_node");
ros::NodeHandle nh;
ros::Rate loop_rate(30.0);
unsigned int loop_count = 1;
double cycle_time_median = 0.0;
MotionAdaption motion_adaption;
while (nh.ok())
{
motion_adaption.adapt();
loop_rate.sleep();
cycle_time_median = ((cycle_time_median * (loop_count - 1)) + loop_rate.cycleTime().toSec()) / loop_count;
ROS_DEBUG_THROTTLE(1.0, "motion_adaption: cycle time %f and median cycle time %f", loop_rate.cycleTime().toSec(),
cycle_time_median);
loop_count++;
}
return 0;
}
/// \brief Returns if we're at the specified position.
bool at(const geometry_msgs::PoseStamped& ref)
{
tf::Vector3 p, ref_p;
tf::Stamped<tf::Pose> hp_tmp, hp;
tf::poseStampedMsgToTF(hand_pose_, hp_tmp);
try {
tf_.transformPose(ref.header.frame_id, hp_tmp, hp);
} catch (tf::TransformException)
{
ROS_WARN("Can't transform hand pose.");
return false;
}
tf::pointMsgToTF(ref.pose.position, ref_p);
double dist = (hp.getOrigin() - ref_p).length();
ROS_DEBUG_THROTTLE(1, "dist: %f", dist);
if (dist < 0.005) // large epsilon.
return true;
else
return false;
}
void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
{
if (joint_state->name.size() != joint_state->position.size())
{
ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state");
return;
}
// read the received values, and update their time stamps
std::size_t n = joint_state->name.size();
std::map<std::string, double> joint_state_map;
const std::map<std::string, std::pair<double, double> > &bounds = kmodel_->getAllVariableBounds();
for (std::size_t i = 0 ; i < n ; ++i)
{
joint_state_map[joint_state->name[i]] = joint_state->position[i];
joint_time_[joint_state->name[i]] = joint_state->header.stamp;
// continuous joints wrap, so we don't modify them (even if they are outside bounds!)
const robot_model::JointModel* jm = kmodel_->getJointModel(joint_state->name[i]);
if (jm && jm->getType() == robot_model::JointModel::REVOLUTE)
if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
continue;
std::map<std::string, std::pair<double, double> >::const_iterator bi = bounds.find(joint_state->name[i]);
// if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within bounds
if (bi != bounds.end())
{
if (joint_state->position[i] < bi->second.first && joint_state->position[i] >= bi->second.first - error_)
joint_state_map[joint_state->name[i]] = bi->second.first;
else
if (joint_state->position[i] > bi->second.second && joint_state->position[i] <= bi->second.second + error_)
joint_state_map[joint_state->name[i]] = bi->second.second;
}
}
bool set_map_values = true;
// read root transform, if needed
if (tf_ && (root_->getType() == robot_model::JointModel::PLANAR ||
root_->getType() == robot_model::JointModel::FLOATING))
{
const std::string &child_frame = root_->getJointModel()->getChildLinkModel()->getName();
const std::string &parent_frame = kmodel_->getModelFrame();
std::string err;
ros::Time tm;
tf::StampedTransform transf;
bool ok = false;
if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
{
try
{
tf_->lookupTransform(parent_frame, child_frame, tm, transf);
ok = true;
}
catch(tf::TransformException& ex)
{
ROS_ERROR_THROTTLE(1, "Unable to lookup transform from %s to %s. Exception: %s", parent_frame.c_str(), child_frame.c_str(), ex.what());
}
}
else
ROS_DEBUG_THROTTLE(1, "Unable to lookup transform from %s to %s: no common time.", parent_frame.c_str(), child_frame.c_str());
if (ok)
{
const std::vector<std::string> &vars = root_->getJointModel()->getVariableNames();
for (std::size_t j = 0; j < vars.size() ; ++j)
joint_time_[vars[j]] = tm;
set_map_values = false;
Eigen::Affine3d eigen_transf;
tf::transformTFToEigen(transf, eigen_transf);
boost::mutex::scoped_lock slock(state_update_lock_);
root_->setVariableValues(eigen_transf);
kstate_.setStateValues(joint_state_map);
current_state_time_ = joint_state->header.stamp;
}
}
if (set_map_values)
{
boost::mutex::scoped_lock slock(state_update_lock_);
kstate_.setStateValues(joint_state_map);
current_state_time_ = joint_state->header.stamp;
}
// callbacks, if needed
for (std::size_t i = 0 ; i < update_callbacks_.size() ; ++i)
update_callbacks_[i](joint_state);
}
bool MotionAdaption::adaptShoulders()
{
// positions
try
{
tf_listener_.waitForTransform("/fixed_ref_frame", robot_r_shoulder_str_, ros::Time(0),
ros::Duration(wait_for_tf_));
tf_listener_.lookupTransform("/fixed_ref_frame", robot_r_shoulder_str_, ros::Time(0),
tf_robot_torso_r_shoulder_);
tf_listener_.waitForTransform("/fixed_ref_frame", robot_l_shoulder_str_, ros::Time(0),
ros::Duration(wait_for_tf_));
tf_listener_.lookupTransform("/fixed_ref_frame", robot_l_shoulder_str_, ros::Time(0),
tf_robot_torso_l_shoulder_);
tf_listener_.waitForTransform(robot_r_shoulder_str_, robot_l_shoulder_str_, ros::Time(0),
ros::Duration(wait_for_tf_));
tf_listener_.lookupTransform(robot_r_shoulder_str_, robot_l_shoulder_str_, ros::Time(0),
tf_robot_r_shoulder_l_shoulder_);
tf_listener_.waitForTransform(robot_r_shoulder_str_, robot_r_elbow_str_, ros::Time(0), ros::Duration(wait_for_tf_));
tf_listener_.lookupTransform(robot_r_shoulder_str_, robot_r_elbow_str_, ros::Time(0), tf_robot_r_shoulder_r_elbow_);
tf_listener_.waitForTransform(robot_r_elbow_str_, robot_r_hand_str_, ros::Time(0), ros::Duration(wait_for_tf_));
tf_listener_.lookupTransform(robot_r_elbow_str_, robot_r_hand_str_, ros::Time(0), tf_robot_r_elbow_r_hand_);
}
catch (tf::TransformException const &ex)
{
ROS_DEBUG("%s",ex.what());
ROS_WARN("(Step 3.4.1) Couldn't get one or more transformations of the robot to calculate body measurements.");
ROS_WARN("No further processing will be done!");
return false;
}
robot_shoulder_heigth_ = sqrt(pow(tf_robot_torso_r_shoulder_.getOrigin()[
tf_robot_torso_r_shoulder_.getOrigin().absolute().maxAxis()], 2));
robot_shoulder_width_ = sqrt(pow(tf_robot_r_shoulder_l_shoulder_.getOrigin()[
tf_robot_r_shoulder_l_shoulder_.getOrigin().absolute().maxAxis()], 2));
robot_upper_arm_length_ = sqrt(tf_robot_r_shoulder_r_elbow_.getOrigin().length2());
robot_lower_arm_length_ = sqrt(tf_robot_r_elbow_r_hand_.getOrigin().length2());
robot_arm_length_ = robot_upper_arm_length_ + robot_lower_arm_length_;
ROS_DEBUG_THROTTLE(1.0, "robot_shoulder_heigth = %f, robot_shoulder_width = %f, robot_arm_length = %f",
robot_shoulder_heigth_, robot_shoulder_width_, robot_arm_length_);
tf_r_shoulder_scaled_.setOrigin(tf_robot_torso_r_shoulder_.getOrigin());
tf_l_shoulder_scaled_.setOrigin(tf_robot_torso_l_shoulder_.getOrigin());
// tf_broadcaster_.sendTransform(tf::StampedTransform(tf_r_shoulder_scaled_, ros::Time::now(),ref_frame, "/r_robot_shoulder"));
internal_tf.setTransform(tf::StampedTransform(tf_r_shoulder_scaled_, calc_time,ref_frame, "/r_robot_shoulder"));
// tf_broadcaster_.sendTransform(tf::StampedTransform(tf_l_shoulder_scaled_, ros::Time::now(),ref_frame, "/l_robot_shoulder"));
internal_tf.setTransform(tf::StampedTransform(tf_l_shoulder_scaled_, calc_time,ref_frame, "/l_robot_shoulder"));
/*
tf_r_shoulder_scaled_.setOrigin(tf::Vector3(robot_shoulder_width_*0;.5, robot_shoulder_heigth_,
robot_shoulder_x_offset_));
tf_l_shoulder_scaled_.setOrigin(tf::Vector3(-robot_shoulder_width_*0.5, robot_shoulder_heigth_,
robot_shoulder_x_offset_));
tf_broadcaster_.sendTransform(tf::StampedTransform(tf_r_shoulder_scaled_, ros::Time::now(),
ref_frame, "/r_shoulder_scaled"));
tf_broadcaster_.sendTransform(tf::StampedTransform(tf_l_shoulder_scaled_, ros::Time::now(),
ref_frame, "/l_shoulder_scaled"));
*/
// orientations
try
{
//tf_listener_.waitForTransform("/r_robot_shoulder", "/r_elbow_scaled", ros::Time(0), ros::Duration(wait_for_tf_));
internal_tf.lookupTransform("/r_robot_shoulder", "/r_elbow_scaled", ros::Time(0), tf_r_shoulder_elbow_);
//tf_listener_.waitForTransform("/r_robot_shoulder", "/r_hand_scaled", ros::Time(0), ros::Duration(wait_for_tf_));
internal_tf.lookupTransform("/r_robot_shoulder", "/r_hand_scaled", ros::Time(0), tf_r_shoulder_hand_);
//tf_listener_.waitForTransform("/l_robot_shoulder", "/l_elbow_scaled", ros::Time(0), ros::Duration(wait_for_tf_));
internal_tf.lookupTransform("/l_robot_shoulder", "/l_elbow_scaled", ros::Time(0), tf_l_shoulder_elbow_);
//tf_listener_.waitForTransform("/l_robot_shoulder", "/l_hand_scaled", ros::Time(0), ros::Duration(wait_for_tf_));
internal_tf.lookupTransform("/l_robot_shoulder", "/l_hand_scaled", ros::Time(0), tf_l_shoulder_hand_);
}
catch (tf::TransformException const &ex)
{
ROS_DEBUG("%s",ex.what());
ROS_WARN("(Step 3.4.2) Couldn't get one or more transformations from the shoulders to the hands and elbows.");
ROS_WARN("No further processing will be done!");
return false;
}
// right shoulder
vec_shoulder_elbow_ = tf_r_shoulder_elbow_.getOrigin();
vec_shoulder_hand_ = tf_r_shoulder_hand_.getOrigin();
if(vec_shoulder_hand_.angle(vec_shoulder_elbow_) < 0.15708 && !vec_r_elbow_hand_valid_.isZero())
{
ROS_DEBUG_THROTTLE(0.1, "valid right hand vector is used (angle between vectors: %f).",
vec_shoulder_hand_.angle(vec_shoulder_elbow_));
vec_shoulder_hand_ = vec_shoulder_elbow_ + vec_r_elbow_hand_valid_;
r_elbow_extended_ = true;
}
else
{
vec_r_elbow_hand_valid_ = vec_shoulder_hand_ - vec_shoulder_elbow_;
r_elbow_extended_ = false;
}
vec_normal_ = vec_shoulder_hand_.cross(vec_shoulder_elbow_);
vec_helper_ = vec_normal_.cross(vec_shoulder_hand_);
vec_shoulder_hand_.normalize();
vec_helper_.normalize();
vec_normal_.normalize();
mat_orientation_.setValue(vec_shoulder_hand_.x(), vec_helper_.x(), vec_normal_.x(),
vec_shoulder_hand_.y(), vec_helper_.y(), vec_normal_.y(),
vec_shoulder_hand_.z(), vec_helper_.z(), vec_normal_.z());
tf_r_shoulder_goal_.setBasis(mat_orientation_);
// tf_broadcaster_.sendTransform(tf::StampedTransform(tf_r_shoulder_goal_, ros::Time::now(), "/r_robot_shoulder", "/r_shoulder_adapted"));
internal_tf.setTransform(tf::StampedTransform(tf_r_shoulder_goal_, calc_time, "/r_robot_shoulder", "/r_shoulder_adapted"));
// left shoulder
vec_shoulder_elbow_ = tf_l_shoulder_elbow_.getOrigin();
vec_shoulder_hand_ = tf_l_shoulder_hand_.getOrigin();
if(vec_shoulder_hand_.angle(vec_shoulder_elbow_) < 0.15708 && !vec_l_elbow_hand_valid_.isZero())
{
ROS_DEBUG_THROTTLE(0.1, "valid right hand vector is used (angle between vectors: %f).",
vec_shoulder_hand_.angle(vec_shoulder_elbow_));
vec_shoulder_hand_ = vec_shoulder_elbow_ + vec_l_elbow_hand_valid_;
l_elbow_extended_ = true;
}
else
{
vec_l_elbow_hand_valid_ = vec_shoulder_hand_ - vec_shoulder_elbow_;
l_elbow_extended_ = false;
}
vec_normal_ = vec_shoulder_hand_.cross(vec_shoulder_elbow_);
vec_helper_ = vec_normal_.cross(vec_shoulder_hand_);
vec_shoulder_hand_.normalize();
vec_helper_.normalize();
vec_normal_.normalize();
mat_orientation_.setValue(vec_shoulder_hand_.x(), vec_helper_.x(), vec_normal_.x(),
vec_shoulder_hand_.y(), vec_helper_.y(), vec_normal_.y(),
vec_shoulder_hand_.z(), vec_helper_.z(), vec_normal_.z());
tf_l_shoulder_goal_.setBasis(mat_orientation_);
// tf_broadcaster_.sendTransform(tf::StampedTransform(tf_l_shoulder_goal_, ros::Time::now(), "/l_robot_shoulder", "/l_shoulder_adapted"));
internal_tf.setTransform(tf::StampedTransform(tf_l_shoulder_goal_, calc_time, "/l_robot_shoulder", "/l_shoulder_adapted"));
return true;
}
void timerCB(const ros::TimerEvent&)
{
ROS_DEBUG_THROTTLE(1, "State: %i", (int)state_);
switch (state_)
{
case IDLE:
// Do nothing.
break;
case GO:
// Always transition to OPEN_HAND, send an open command if
// necessary.
if (!handOpened())
{
openHand();
}
state_ = OPEN_HAND;
break;
case OPEN_HAND:
if (handOpened())
{
// Transition to toward pose.
pubGoal(fixed_toward_pose_);
state_ = GO_TOWARD;
}
break;
case GO_TOWARD:
if (at(fixed_toward_pose_))
{
// Transition to target pose.
pubGoal(fixed_target_pose_);
state_ = LOWER;
}
break;
case LOWER:
if (at(fixed_target_pose_))
{
// Transition to hand closing.
closeHand();
state_ = CLOSE_HAND;
}
break;
case CLOSE_HAND:
if (!handOpened())
{
// Transition to lifted pose.
pubGoal(fixed_lifted_pose_);
state_ = LIFT;
}
break;
case LIFT:
if (at(fixed_lifted_pose_))
{
// Transition back to idle.
state_ = IDLE;
}
break;
default:
state_ = IDLE;
break;
};
}
void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
{
if (joint_state->name.size() != joint_state->position.size())
{
ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of positions)");
return;
}
bool update = false;
{
boost::mutex::scoped_lock _(state_update_lock_);
// read the received values, and update their time stamps
std::size_t n = joint_state->name.size();
current_state_time_ = joint_state->header.stamp;
for (std::size_t i = 0 ; i < n ; ++i)
{
const robot_model::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
if (!jm)
continue;
// ignore fixed joints, multi-dof joints (they should not even be in the message)
if (jm->getVariableCount() != 1)
continue;
joint_time_[joint_state->name[i]] = joint_state->header.stamp;
if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
{
update = true;
robot_state_.setJointPositions(jm, &(joint_state->position[i]));
// continuous joints wrap, so we don't modify them (even if they are outside bounds!)
if (jm->getType() == robot_model::JointModel::REVOLUTE)
if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
continue;
const robot_model::VariableBounds &b = jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds
// if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within bounds
if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
robot_state_.setJointPositions(jm, &b.min_position_);
else
if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
robot_state_.setJointPositions(jm, &b.max_position_);
}
}
// read root transform, if needed
if (tf_ && (robot_model_->getRootJoint()->getType() == robot_model::JointModel::PLANAR ||
robot_model_->getRootJoint()->getType() == robot_model::JointModel::FLOATING))
{
const std::string &child_frame = robot_model_->getRootLink()->getName();
const std::string &parent_frame = robot_model_->getModelFrame();
std::string err;
ros::Time tm;
tf::StampedTransform transf;
bool ok = false;
if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
{
try
{
tf_->lookupTransform(parent_frame, child_frame, tm, transf);
ok = true;
}
catch(tf::TransformException& ex)
{
ROS_ERROR_THROTTLE(1, "Unable to lookup transform from %s to %s. Exception: %s", parent_frame.c_str(), child_frame.c_str(), ex.what());
}
}
else
ROS_DEBUG_THROTTLE(1, "Unable to lookup transform from %s to %s: no common time.", parent_frame.c_str(), child_frame.c_str());
if (ok && last_tf_update_ != tm)
{
update = true;
last_tf_update_ = tm;
const std::vector<std::string> &vars = robot_model_->getRootJoint()->getVariableNames();
for (std::size_t j = 0; j < vars.size() ; ++j)
joint_time_[vars[j]] = tm;
Eigen::Affine3d eigen_transf;
tf::transformTFToEigen(transf, eigen_transf);
robot_state_.setJointPositions(robot_model_->getRootJoint(), eigen_transf);
}
}
}
// callbacks, if needed
if (update)
for (std::size_t i = 0 ; i < update_callbacks_.size() ; ++i)
update_callbacks_[i](joint_state);
}
int main(int argc, char **argv)
{
float publish_rate = 1;
float shutdown_after_sending = 10;
float message_payload_size = 0;
long int sent_count = 0;
char* message_string;
ros::init(argc, argv, "talker");
ros::NodeHandle nh;
pubsub::latency_test_message msg;
ros::Publisher chatter_pub = nh.advertise<pubsub::latency_test_message>("chatter", 1000);
ros::param::get("~/publish_rate", publish_rate);
ros::param::get("~/shutdown_after_sending", shutdown_after_sending);
ros::param::get("~/message_payload_size", message_payload_size);
if (message_payload_size > 0) {
message_string = (char *)malloc((size_t)message_payload_size);
for (long int i=0; i < message_payload_size; i++) {
message_string[i] = i%10 + '0';
}
msg.payload.append(message_string);
free(message_string);
}
ros::Rate loop_rate((double)publish_rate);
ROS_INFO("Publisher ('talker') loop starts now with publish rate %ld", (long int)publish_rate);
ROS_INFO("Publisher ('talker') will shutdown after publishing %ld messages", (long int)shutdown_after_sending);
ROS_INFO("Publisher ('talker') will publish messages with a size of %ld bytes", (long int)message_payload_size);
while (ros::ok() && sent_count < (long int)shutdown_after_sending)
{
msg.header.seq = sent_count;
// begin vmem size code
// source copied from MIRA benchmark and adapted for re-use here
std::ifstream statStream("/proc/self/status", std::ios_base::in);
std::string line;
bool vmem_found = false;
while(!std::getline(statStream, line).eof() && !vmem_found)
{
// format of line we are looking for:
// VmSize: 28812 kB
if (line.find("VmSize") != std::string::npos)
{
vmem_found = true;
sscanf(line.c_str(),"VmSize:%ld kB", &(msg.virtual_memory_size));
// important note: a throttled console message uses quite some extra cpu power, so
// be aware of that when you turn debug on using e.g. rqt_logger_level
ROS_DEBUG_THROTTLE(1.0,">>>> sscanf of line %s gives pub virtual_memory_size %ld",
line.c_str(), msg.virtual_memory_size);
}
}
// end vmem size code
ros::WallTime wallTime = ros::WallTime::now();
msg.header.stamp.sec = wallTime.sec;
msg.header.stamp.nsec = wallTime.nsec;
chatter_pub.publish(msg);
sent_count++;
loop_rate.sleep();
}
ros::shutdown();
return 0;
} //main
/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Jaco Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void JacoArm::publishJointAngles(void)
{
FingerAngles fingers;
jaco_comm_.getFingerPositions(fingers);
// Query arm for current joint angles
JacoAngles current_angles;
jaco_comm_.getJointAngles(current_angles);
jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg(robot_type);
jaco_angles.joint1 = current_angles.Actuator1;
jaco_angles.joint2 = current_angles.Actuator2;
jaco_angles.joint3 = current_angles.Actuator3;
jaco_angles.joint4 = current_angles.Actuator4;
jaco_angles.joint5 = current_angles.Actuator5;
jaco_angles.joint6 = current_angles.Actuator6;
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
joint_state.header.stamp = ros::Time::now();
// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
joint_state.position.resize(jaco_joints_count);
double j6o = robot_type == ROBOT_TYPE_JACO ? JACO_JOINT_2_ANGLE : MICO_JOINT_2_ANGLE;
joint_state.position[0] = (180- jaco_angles.joint1) * (PI / 180);
joint_state.position[1] = (jaco_angles.joint2 - j6o) * (PI / 180);
joint_state.position[2] = (90-jaco_angles.joint3) * (PI / 180);
joint_state.position[3] = (180-jaco_angles.joint4) * (PI / 180);
joint_state.position[4] = (180-jaco_angles.joint5) * (PI / 180);
joint_state.position[5] = (j6o-jaco_angles.joint6) * (PI / 180);
if (joint_state.position[5] > PI) {
joint_state.position[5] -= (2*PI);
}
joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
if(robot_type == ROBOT_TYPE_JACO) {
joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;
}
// Joint velocities
JacoAngles current_vels;
jaco_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(jaco_joints_count);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
ROS_DEBUG_THROTTLE(0.1,
"Raw joint velocities: %f %f %f %f %f %f",
joint_state.velocity[0],
joint_state.velocity[1],
joint_state.velocity[2],
joint_state.velocity[3],
joint_state.velocity[4],
joint_state.velocity[5]);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// No velocity for the fingers:
joint_state.velocity[6] = 0.0;
joint_state.velocity[7] = 0.0;
if(robot_type==ROBOT_TYPE_JACO){
joint_state.velocity[8] = 0.0;
}
// Joint torques (effort)
// NOTE: Currently invalid.
JacoAngles joint_tqs;
joint_state.effort.resize(jaco_joints_count);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
joint_state.effort[6] = 0.0;
joint_state.effort[7] = 0.0;
if(robot_type==ROBOT_TYPE_JACO){
joint_state.effort[8] = 0.0;
}
ROS_DEBUG_THROTTLE(0.1,
"Raw joint torques: %f %f %f %f %f %f",
joint_state.effort[0],
joint_state.effort[1],
joint_state.effort[2],
joint_state.effort[3],
joint_state.effort[4],
joint_state.effort[5]);
joint_angles_publisher_.publish(jaco_angles);
joint_state_publisher_.publish(joint_state);
}
puppeteer_msgs::Robots calibrate_routine(void)
{
ROS_DEBUG("calibration_routine triggered");
static Eigen::MatrixXd cal_eig(nr,3);
puppeteer_msgs::Robots sorted_bots;
sorted_bots.robots.resize(nr);
// if this is the first call to the function, let's get
// the starting pose for each of the robots.
if(calibrate_count == 0)
{
ROS_DEBUG_THROTTLE(1,"Calibrating...");
puppeteer_msgs::Robots r;
double tmp;
r.robots.resize(nr);
robot_start_ori.clear();
for (int j=0; j<nr; j++)
{
std::stringstream ss;
ss << "/robot_" << j+1 << "/robot_x0";
ros::param::get(ss.str(), tmp);
ss.str(""); ss.clear();
r.robots[j].point.x = tmp;
ss << "/robot_" << j+1 << "/robot_y0";
ros::param::get(ss.str(), tmp);
ss.str(""); ss.clear();
r.robots[j].point.y = tmp;
ss << "/robot_" << j+1 << "/robot_z0";
ros::param::get(ss.str(), tmp);
ss.str(""); ss.clear();
r.robots[j].point.z = tmp;
ss << "/robot_" << j+1 << "/robot_th0";
ros::param::get(ss.str(), tmp);
robot_start_ori.push_back(tmp);
}
start_bots = r;
// increment counter, and initialize transform values
calibrate_count++;
cal_pos << 0, 0, 0;
for(int i=0; i<cal_eig.rows(); i++) {
cal_eig(i,0) = 0; cal_eig(i,1) = 0; cal_eig(i,2) = 0; }
return sorted_bots;
}
// we are in the process of calibrating:
else if (calibrate_count <= NUM_CALIBRATES)
{
ROS_DEBUG("summing the data");
sorted_bots = sort_bots_with_order(¤t_bots);
Eigen::Matrix<double, Eigen::Dynamic, 3> sorted_eig;
bots_to_eigen(&sorted_eig, &sorted_bots);
// now we have the sorted matrix... let's keep on
// adding the values:
cal_eig += sorted_eig;
calibrate_count++;
}
// we are ready to find the transformation:
else
{
ROS_DEBUG("Getting transforms");
cal_eig /= (double) NUM_CALIBRATES; // average all vectors
// get transform for each robot:
sorted_bots = sort_bots_with_order(¤t_bots);
Eigen::Matrix<double, Eigen::Dynamic, 3> temp_eig;
bots_to_eigen(&temp_eig, &start_bots);
cal_eig = temp_eig-cal_eig;
// Now find the mean of the transforms:
for (int i=0; i<nr; i++)
cal_pos += cal_eig.block<1,3>(i,0);
cal_pos /= nr;
cal_pos = -1.0*cal_pos;
ROS_DEBUG("calibration pose: %f, %f, %f",
cal_pos(0),cal_pos(1),cal_pos(2));
calibrated_flag = true;
calibrate_count = 0;
}
return sorted_bots;
}
void ExploreFrontier::findFrontiers() {
frontiers_.clear();
const size_t w = costmap_->getSizeInCellsX();
const size_t h = costmap_->getSizeInCellsY();
const size_t size = w * h;
map_.info.width = w;
map_.info.height = h;
map_.data.resize(size);
map_.info.resolution = costmap_->getResolution();
map_.info.origin.position.x = costmap_->getOriginX();
map_.info.origin.position.y = costmap_->getOriginY();
// lock as we are accessing raw underlying map
auto *mutex = costmap_->getMutex();
std::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap_->getCharMap();
ROS_DEBUG_COND(!map, "no map available");
for (size_t i = 0; i < size; ++i) {
// //get the world point for the index
// unsigned int mx, my;
// costmap.indexToCells(i, mx, my);
// geometry_msgs::Point p;
// costmap.mapToWorld(mx, my, p.x, p.y);
//
//check if the point has valid potential and is next to unknown space
// bool valid_point = planner_->validPointPotential(p);
bool valid_point = (map[i] < costmap_2d::LETHAL_OBSTACLE);
// ROS_DEBUG_COND(!valid_point, "invalid point %u", map[i]);
if ((valid_point && map) &&
(((i+1 < size) && (map[i+1] == costmap_2d::NO_INFORMATION)) ||
((i-1 < size) && (map[i-1] == costmap_2d::NO_INFORMATION)) ||
((i+w < size) && (map[i+w] == costmap_2d::NO_INFORMATION)) ||
((i-w < size) && (map[i-w] == costmap_2d::NO_INFORMATION))))
{
ROS_DEBUG_THROTTLE(30, "found suitable point");
map_.data[i] = -128;
} else {
map_.data[i] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
for (size_t y = 0, i = h-1; y < w; ++y) {
ROS_DEBUG_THROTTLE(30, "cleaning cell %lu", i);
map_.data[i] = -127;
i += h;
}
// Group adjoining map_ pixels
signed char segment_id = 127;
std::vector<std::vector<FrontierPoint>> segments;
for (size_t i = 0; i < size; i++) {
if (map_.data[i] == -128) {
ROS_DEBUG_THROTTLE(30, "adjoining on %lu", i);
std::vector<size_t> neighbors;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
// claim all neighbors
while (!neighbors.empty()) {
ROS_DEBUG_THROTTLE(30, "got %lu neighbors", neighbors.size());
size_t idx = neighbors.back();
neighbors.pop_back();
map_.data[idx] = segment_id;
tf::Vector3 tot(0,0,0);
size_t c = 0;
if (idx+1 < size && map[idx+1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(1,0,0);
++c;
}
if (idx-1 < size && map[idx-1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(-1,0,0);
++c;
}
if (idx+w < size && map[idx+w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,1,0);
++c;
}
if (idx-w < size && map[idx-w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,-1,0);
++c;
}
if(!(c > 0)) {
ROS_ERROR("assertion failed. corrupted costmap?");
ROS_DEBUG("c is %lu", c);
continue;
}
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (idx-1 < size && map_.data[idx-1] == -128)
neighbors.push_back(idx-1);
if (idx+1 < size && map_.data[idx+1] == -128)
neighbors.push_back(idx+1);
if (idx-w < size && map_.data[idx-w] == -128)
neighbors.push_back(idx-w);
if (idx-w+1 < size && map_.data[idx-w+1] == -128)
neighbors.push_back(idx-w+1);
if (idx-w-1 < size && map_.data[idx-w-1] == -128)
neighbors.push_back(idx-w-1);
if (idx+w < size && map_.data[idx+w] == -128)
neighbors.push_back(idx+w);
if (idx+w+1 < size && map_.data[idx+w+1] == -128)
neighbors.push_back(idx+w+1);
if (idx+w-1 < size && map_.data[idx+w-1] == -128)
neighbors.push_back(idx+w-1);
}
segments.push_back(std::move(segment));
segment_id--;
if (segment_id < -127)
break;
}
}
// no longer accessing map
lock.unlock();
int num_segments = 127 - segment_id;
if (num_segments <= 0) {
ROS_DEBUG("#segments is <0, no frontiers found");
return;
}
for (auto& segment : segments) {
Frontier frontier;
size_t segment_size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
if (segment_size * costmap_->getResolution() < costmap_client_->getInscribedRadius()) {
ROS_DEBUG_THROTTLE(30, "some frontiers were small");
continue;
}
float x = 0, y = 0;
tf::Vector3 d(0,0,0);
for (const auto& frontier_point : segment) {
d += frontier_point.d;
size_t idx = frontier_point.idx;
x += (idx % w);
y += (idx / w);
}
d = d / segment_size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / segment_size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / segment_size);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(d.y(), d.x()));
frontier.size = segment_size;
frontiers_.push_back(std::move(frontier));
}
}
/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Jaco Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void JacoArm::publishJointAngles(void)
{
FingerAngles fingers;
jaco_comm_.getFingerPositions(fingers);
// Query arm for current joint angles
JacoAngles current_angles;
jaco_comm_.getJointAngles(current_angles);
jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
// joint_state.header.stamp = ros::Time::now();
sensor_msgs::JointState joint_position_state;
joint_position_state.name = joint_urdf_names_;
joint_position_state.header.stamp = ros::Time::now();
//jaco_angles.joint1 = current_angles.Actuator1;
//jaco_angles.joint2 = current_angles.Actuator2;
//jaco_angles.joint3 = current_angles.Actuator3;
//jaco_angles.joint4 = current_angles.Actuator4;
//jaco_angles.joint5 = current_angles.Actuator5;
//jaco_angles.joint6 = current_angles.Actuator6;
// Transform from physical angles to Kinova DH algorithm in radians , then place into vector array
joint_state.position.resize(9);
joint_state.position[0] = jaco_angles.joint1;
joint_state.position[1] = jaco_angles.joint2;
joint_state.position[2] = jaco_angles.joint3;
joint_state.position[3] = jaco_angles.joint4;
joint_state.position[4] = jaco_angles.joint5;
joint_state.position[5] = jaco_angles.joint6;
joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;
// just for visualization
joint_position_state.position.resize(6);
joint_position_state.position[0] = jaco_angles.joint1;
joint_position_state.position[1] = jaco_angles.joint2;
joint_position_state.position[2] = jaco_angles.joint3;
joint_position_state.position[3] = jaco_angles.joint4;
joint_position_state.position[4] = jaco_angles.joint5;
joint_position_state.position[5] = jaco_angles.joint6;
// Joint velocities
JacoAngles current_vels;
jaco_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(9);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
ROS_DEBUG_THROTTLE(0.1,
"Raw joint velocities: %f %f %f %f %f %f",
joint_state.velocity[0],
joint_state.velocity[1],
joint_state.velocity[2],
joint_state.velocity[3],
joint_state.velocity[4],
joint_state.velocity[5]);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// No velocity for the fingers:
joint_state.velocity[6] = 0.0;
joint_state.velocity[7] = 0.0;
joint_state.velocity[8] = 0.0;
joint_position_state.velocity.resize(6);
joint_position_state.velocity[0] = current_vels.Actuator1;
joint_position_state.velocity[1] = current_vels.Actuator2;
joint_position_state.velocity[2] = current_vels.Actuator3;
joint_position_state.velocity[3] = current_vels.Actuator4;
joint_position_state.velocity[4] = current_vels.Actuator5;
joint_position_state.velocity[5] = current_vels.Actuator6;
// Joint torques (effort)
// NOTE: Currently invalid.
JacoAngles joint_tqs;
joint_state.effort.resize(9);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
joint_state.effort[6] = 0.0;
joint_state.effort[7] = 0.0;
joint_state.effort[8] = 0.0;
ROS_DEBUG_THROTTLE(0.1,
"Raw joint torques: %f %f %f %f %f %f",
joint_state.effort[0],
joint_state.effort[1],
joint_state.effort[2],
joint_state.effort[3],
joint_state.effort[4],
joint_state.effort[5]);
joint_position_state.effort.resize(6);
joint_position_state.effort[0] = joint_tqs.Actuator1;
joint_position_state.effort[1] = joint_tqs.Actuator2;
joint_position_state.effort[2] = joint_tqs.Actuator3;
joint_position_state.effort[3] = joint_tqs.Actuator4;
joint_position_state.effort[4] = joint_tqs.Actuator5;
joint_position_state.effort[5] = joint_tqs.Actuator6;
joint_angles_publisher_.publish(jaco_angles);
joint_state_publisher_.publish(joint_state);
joint_position_state_publisher_.publish(joint_position_state);
ros::spinOnce();
}
void
Tracker::updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites)
{
if (!sites)
return;
std::list<vpMbtDistanceLine*> linesList;
if(trackerType_!="klt") { // For mbt and hybrid
dynamic_cast<vpMbEdgeTracker*>(tracker_)->getLline(linesList, 0);
std::list<vpMbtDistanceLine*>::iterator linesIterator = linesList.begin();
bool noVisibleLine = true;
for (; linesIterator != linesList.end(); ++linesIterator)
{
vpMbtDistanceLine* line = *linesIterator;
#if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
if (line && line->isVisible() && ! line->meline.empty())
#else
if (line && line->isVisible() && line->meline)
#endif
{
#if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
for(unsigned int a = 0 ; a < line->meline.size() ; a++)
{
if(line->meline[a] != NULL) {
std::list<vpMeSite>::const_iterator sitesIterator = line->meline[a]->getMeList().begin();
if (line->meline[a]->getMeList().empty())
ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
for (; sitesIterator != line->meline[a]->getMeList().end(); ++sitesIterator)
{
#elif VISP_VERSION_INT >= VP_VERSION_INT(2,10,0) // ViSP >= 2.10.0
std::list<vpMeSite>::const_iterator sitesIterator = line->meline->getMeList().begin();
if (line->meline->getMeList().empty())
ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
for (; sitesIterator != line->meline->getMeList().end(); ++sitesIterator)
{
#else
std::list<vpMeSite>::const_iterator sitesIterator = line->meline->list.begin();
if (line->meline->list.empty())
ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
for (; sitesIterator != line->meline->list.end(); ++sitesIterator)
{
#endif
visp_tracker::MovingEdgeSite movingEdgeSite;
movingEdgeSite.x = sitesIterator->ifloat;
movingEdgeSite.y = sitesIterator->jfloat;
#if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) // ViSP < 2.10.0
movingEdgeSite.suppress = sitesIterator->suppress;
#endif
sites->moving_edge_sites.push_back (movingEdgeSite);
}
noVisibleLine = false;
}
#if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
}
}
#endif
}
if (noVisibleLine)
ROS_DEBUG_THROTTLE(10, "no distance lines");
}
}
void
Tracker::updateKltPoints(visp_tracker::KltPointsPtr klt)
{
if (!klt)
return;
#if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) // ViSP < 2.10.0
vpMbHiddenFaces<vpMbtKltPolygon> *poly_lst;
std::map<int, vpImagePoint> *map_klt;
if(trackerType_!="mbt") { // For klt and hybrid
poly_lst = &dynamic_cast<vpMbKltTracker*>(tracker_)->getFaces();
for(unsigned int i = 0 ; i < poly_lst->size() ; i++)
{
if((*poly_lst)[i])
{
map_klt = &((*poly_lst)[i]->getCurrentPoints());
if(map_klt->size() > 3)
{
for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
{
visp_tracker::KltPoint kltPoint;
kltPoint.id = it->first;
kltPoint.i = it->second.get_i();
kltPoint.j = it->second.get_j();
klt->klt_points_positions.push_back (kltPoint);
}
}
}
}
}
#else // ViSP >= 2.10.0
std::list<vpMbtDistanceKltPoints*> *poly_lst;
std::map<int, vpImagePoint> *map_klt;
if(trackerType_!="mbt") { // For klt and hybrid
poly_lst = &dynamic_cast<vpMbKltTracker*>(tracker_)->getFeaturesKlt();
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=poly_lst->begin(); it!=poly_lst->end(); ++it){
map_klt = &((*it)->getCurrentPoints());
if((*it)->polygon->isVisible()){
if(map_klt->size() > 3)
{
for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
{
visp_tracker::KltPoint kltPoint;
kltPoint.id = it->first;
kltPoint.i = it->second.get_i();
kltPoint.j = it->second.get_j();
klt->klt_points_positions.push_back (kltPoint);
}
}
}
}
}
#endif
}
void Tracker::checkInputs()
{
ros::V_string topics;
topics.push_back(rectifiedImageTopic_);
checkInputs_.start(topics, 60.0);
}
