BaxterArmMvmtDetector::BaxterArmMvmtDetector(ros::NodeHandle& nodeHandle):
nodeHandler(nodeHandle) {
publishPointToLookAt = nodeHandler.advertise<geometry_msgs::Point>("/pointToLookAt", 100);
ros::Rate loop_rate(5);
lookupTransform("/head_camera", "/left_hand", ros::Time(0), tfLeftArmPast);
lookupTransform("/head_camera", "/right_hand", ros::Time(0), tfRightArmPast);
geometry_msgs::Point msg;
while (nodeHandler.ok()) {
lookupTransform("/head_camera", "/left_hand", ros::Time(0), tfLeftArmNow);
lookupTransform("/head_camera", "/right_hand", ros::Time(0), tfRightArmNow);
Point originLeftNow = Point(tfLeftArmNow.getOrigin(), 100.0);
Point originLeftPast = Point(tfLeftArmPast.getOrigin(), 100.0);
Point originRightNow = Point(tfRightArmNow.getOrigin(), 100.0);
Point originRightPast = Point(tfRightArmPast.getOrigin(), 100.0);
msg = Point().toGeometryMessage();
if (originLeftNow != originLeftPast && originRightNow != originRightPast) {
//movement of both arms
} else if (originLeftNow != originLeftPast) {
//movement of left arm
msg = originLeftNow.toGeometryMessage();
} else if (originRightNow != originRightPast) {
//movement of right arm
msg = originRightNow.toGeometryMessage();
}
tfLeftArmPast = tfLeftArmNow;
tfRightArmPast = tfRightArmNow;
publishPointToLookAt.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}
TransformerTF2::FuturePtr TransformerTF2::requestTransform(
const std::string& target_frame, const std::string& source_frame,
const boost::posix_time::ptime& time) {
FuturePtr result(new FutureType());
boost::mutex::scoped_lock lock(inprogressMutex);
try {
Transform t = lookupTransform(target_frame, source_frame, time);
result->set(t);
RSCTRACE(this->logger, "Lookup possible before request applies. Take shortcut.");
} catch (tf2::LookupException &e) {
RSCTRACE(this->logger, "Lookup NOT possible before request applies. Register request.");
this->requestsInProgress.insert(
std::make_pair(Request(target_frame, source_frame, ros::Time().fromBoost(time)),
result));
} catch (tf2::ExtrapolationException &e) {
RSCTRACE(this->logger, "Lookup NOT possible before request applies. Register request.");
this->requestsInProgress.insert(
std::make_pair(Request(target_frame, source_frame, ros::Time().fromBoost(time)),
result));
}
return result;
}
geometry_msgs::TransformStamped
Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout) const
{
canTransform(target_frame, source_frame, time, timeout);
return lookupTransform(target_frame, source_frame, time);
}
bool ROSKinectBridge::setData(uima::CAS &tcas, uint64_t ts)
{
if(!newData())
{
return false;
}
MEASURE_TIME;
cv::Mat color, depth;
sensor_msgs::CameraInfo cameraInfo, cameraInfoHD;
lock.lock();
color = this->color;
depth = this->depth;
cameraInfo = this->cameraInfo;
cameraInfoHD = this->cameraInfoHD;
_newData = false;
lock.unlock();
rs::SceneCas cas(tcas);
lookupTransform(tcas, cameraInfo.header.stamp);
if(color.cols >= 1280)
{
cas.set(VIEW_COLOR_IMAGE_HD, color);
}
else
{
cas.set(VIEW_COLOR_IMAGE, color);
}
if(depth.cols >= 1280)
{
cas.set(VIEW_DEPTH_IMAGE_HD, depth);
}
else
{
cas.set(VIEW_DEPTH_IMAGE, depth);
}
cas.set(VIEW_CAMERA_INFO, cameraInfo);
cas.set(VIEW_CAMERA_INFO_HD, cameraInfoHD);
cas.getScene().timestamp.set(cameraInfo.header.stamp.toNSec());
return true;
}
void TfLookup::alGoalCb(AlServer::GoalHandle gh)
{
if (!gh.getGoal())
{
gh.setCanceled(AlServer::Result(), "something went wrong, goal canceled");
return;
}
gh.setAccepted();
AlServer::GoalConstPtr goal = gh.getGoal();
AlServer::Result r;
if (lookupTransform(goal->target_frame, goal->source_frame,
ros::Time(0), r.transform))
gh.setSucceeded(r);
else
gh.setAborted(r);
}
bool TfLookup::srvLookupTransform(tf_lookup::lookupTransformRequest &req,
tf_lookup::lookupTransformResponse &res)
{
return lookupTransform(req.target_frame, req.source_frame,
req.transform_time, res.transform);
}
