void Explore::transfromRobotToMapPosition(float x_robot, float y_robot, float &x_map, float &y_map){
ros::Time now = ros::Time::now();
tf::StampedTransform tfRO;
tf_listener_.waitForTransform("/base_link", "/odom", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/base_link", "/odom", now, tfRO);
tf::Transform tfOR = tfRO.inverse();
tf::StampedTransform tfOM;
tf_listener_.waitForTransform("/odom", "/map", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/odom", "/map", now, tfOM);
tf::Transform tfMO = tfOM.inverse();
tf::Transform tfRM = tfMO*tfOR;
float noused = 0.0;
tf::Transform tfRD;
tfRD.setOrigin(tf::Vector3(x_robot, y_robot, noused));
tf::Transform tfOD = tfOR * tfRD;
tf::Transform tfMD = tfMO * tfOD;
x_map = tfMD.getOrigin ()[0]; // wspolrzedne docelowe w ukladzie mapy
y_map = tfMD.getOrigin ()[1]; // wspolrzedne docelowe w ukladzie mapy
}
void DepthSensorCallback(const sensor_msgs::PointCloud2ConstPtr msg)
{
// Get the current world space tool position
m_MsgHeaderStamp = msg->header.stamp;
m_Listener.waitForTransform("/world", "VSV/Tool",
m_MsgHeaderStamp, ros::Duration(1.0));
tf::StampedTransform transform;
m_Listener.lookupTransform("/world", "VSV/Tool",
m_MsgHeaderStamp, transform);
m_WorldSpaceToolPosition = transform * tf::Vector3(0.0, 0.0, 0.0);
// Get the current world space robot position
m_Listener.waitForTransform("/world", "VSV/base",
m_MsgHeaderStamp, ros::Duration(1.0));
m_Listener.lookupTransform("/world", "VSV/base",
m_MsgHeaderStamp, transform);
m_WorldSpaceRobotPosition = transform * tf::Vector3(0.0, 0.0, 0.0);
//ROS_INFO("Base position %f %f %f",
// m_WorldSpaceRobotPosition.x(), m_WorldSpaceRobotPosition.y(), m_WorldSpaceRobotPosition.z());
// Get the orientation of the robot
auto Q = transform.getRotation();
tf::Matrix3x3 M;
M.setRotation(Q);
double roll, pitch;
M.getRPY(roll, pitch, m_Orientation);
}
// void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
void callback (const sensor_msgs::PointCloud2ConstPtr& pc, const sensor_msgs::ImageConstPtr& im)
{
if (!cloud_and_image_received_)
{
//Write cloud
//transform to target frame
bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_,
pc->header.stamp, ros::Duration(10.0));
tf::StampedTransform transform;
if (found_transform)
{
//ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform);
ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", pc->header.frame_id.c_str());
}
else
{
ROS_ERROR("No transform for pointcloud found!!!");
return;
}
bag_.write(input_cloud_topic_, pc->header.stamp, *pc);
geometry_msgs::TransformStamped transform_msg;
tf::transformStampedTFToMsg(transform, transform_msg);
bag_.write(input_cloud_topic_ + "/transform", transform_msg.header.stamp, transform_msg);
ROS_INFO("Wrote cloud to %s", bag_name_.c_str());
//Write image
found_transform = tf_.waitForTransform(im->header.frame_id, to_frame_,
im->header.stamp, ros::Duration(10.0));
if (found_transform)
{
//ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
tf_.lookupTransform(to_frame_,im->header.frame_id, im->header.stamp, transform);
ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", im->header.frame_id.c_str());
}
else
{
ROS_ERROR("No transform for image found!!!");
return;
}
bag_.write(input_image_topic_, im->header.stamp, im);
tf::transformStampedTFToMsg(transform, transform_msg);
bag_.write(input_image_topic_ + "/transform", transform_msg.header.stamp, transform_msg);
ROS_INFO("Wrote image to %s", bag_name_.c_str());
cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(input_camera_info_topic_);
bag_.write(input_camera_info_topic_, cam_info_->header.stamp, cam_info_);
ROS_INFO("Wrote Camera Info to %s", bag_name_.c_str());
cloud_and_image_received_ = true;
}
}
bool checkObjectInMap(robot_msgs::checkObjectInMap::Request &req, robot_msgs::checkObjectInMap::Response &res) {
int width = 16;
geometry_msgs::PointStamped tf_object, obj;
obj.point.x = req.point.x;
obj.point.y = req.point.y;
obj.header.frame_id = "camera";
listener.waitForTransform("/camera", "/map", ros::Time(0), ros::Duration(1));
listener.transformPoint("map", obj, tf_object);
int x = floor(tf_object.point.x/resolution);
int y = floor(tf_object.point.y/resolution);
//ROS_INFO("COORDINATE OBJECT x: %d y: %d", x, y);
for(int i = x-width/2; i <= x+width/2; i++)
{
//ROS_INFO("CHECK COORDINATE OBJECT x: %d", i);
for(int j = y-width/2; j <= y+width/2; j++)
{
//ROS_INFO("Value %d", final_map[i + width_map*j]);
if(final_map[i + width_map*j] == -106)
{
res.inMap = true;
return true;
}
}
}
res.inMap = false;
return true;
}
void doubleLaserCallback(const sensor_msgs::LaserScan& scan1, const sensor_msgs::LaserScan& scan2)
{
//transform both to the base frame, and republish this to the laser line extractor.
tf::StampedTransform laser1_to_base;
tf::StampedTransform laser2_to_base;
try{
tf_listener_->waitForTransform("/laser1", "/base_link",scan1.header.stamp, ros::Duration(0.6));
tf_listener_->lookupTransform("/laser1","/base_link",scan1.header.stamp, laser1_to_base);
tf_listener_->waitForTransform("/laser2", "/base_link",scan2.header.stamp, ros::Duration(0.6));
tf_listener_->lookupTransform("/laser2","/base_link",scan2.header.stamp, laser2_to_base);
} catch(tf::TransformException ex){
ROS_ERROR("Laser Merge Callback failure: %s",ex.what());
}
}
Eigen::Matrix4f TrajectoryPublisher::getMatrixForTf(const tf::TransformListener& tfListener,
const std::string& target_frame, const std::string& source_frame,
const ros::Time& time)
{
Eigen::Matrix4f eigenTransform;
tf::StampedTransform tfTransform;
try
{
ros::Time tm;
std::string err;
{
if (tfListener.waitForTransform(target_frame, source_frame, time, ros::Duration(0.2f)))
{
tfListener.lookupTransform(target_frame, source_frame, time, tfTransform);
pcl_ros::transformAsMatrix(tfTransform, eigenTransform);
}
}
}
catch (tf::TransformException ex)
{
ROS_WARN("transofrm error: %s", ex.what());
}
return eigenTransform;
}
void Explore::transFromOdomToMapPosition(float x_odom_pose, float y_odom_pose, float theta,
float &x_map_pose, float &y_map_pose, tf::Quaternion& q){
ros::Time now = ros::Time(0);
tf::StampedTransform tfMO; // odom w map
tf_listener_.waitForTransform("/map", "/odom", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/map", "/odom", now, tfMO);
tf::Transform tfOD;
tf::Quaternion quat;
quat.setRPY(0, 0, theta);
tfOD.setOrigin(tf::Vector3(x_odom_pose, y_odom_pose, 0.0)); // docelowe w odom
tfOD.setRotation( quat );
tf::Transform tfMD = tfMO * tfOD; // docelowe w map
x_map_pose = tfMD.getOrigin ()[0]; // wspolrzedne docelowe w ukladzie map
y_map_pose = tfMD.getOrigin ()[1]; // wspolrzedne docelowe w ukladzie map
q = tfMD.getRotation();
//ROS_INFO("ODOM TO MAP x_odom = %f, y_odom = %f, x_map = %f, y_map = %f", x_odom_pose, y_odom_pose, x_map_pose, y_map_pose);
}
void simulateOneCycle()
{
tf::StampedTransform transform;
try{
listener.waitForTransform("map","EEframe", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform( "map","EEframe", ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
// This is correct, but does not work.
// state_ = transform;
KDL::Frame state_copy;
tf::transformTFToKDL(state_, state_copy);
nh_.getParamCached("/cds_controller_pr2/run_controller", run_controller_flag);
if (run_controller_flag == 1.0){ // Controller running -> Update next desired position
tf::transformKDLToTF(cds_.update(state_copy), state_);
} else {
state_ = transform;
}
broadcastTF();
}
bool getTransform(const std::string& refFrame,
const std::string& childFrame,
tf::StampedTransform& transform)
{
std::string errMsg;
if ( !_tfListener.waitForTransform(refFrame,
childFrame,
ros::Time(0),
ros::Duration(0.5),
ros::Duration(0.01),
&errMsg)
)
{
ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg);
return false;
}
else
{
try
{
_tfListener.lookupTransform( refFrame, childFrame,
ros::Time(0), //get latest available
transform);
}
catch ( const tf::TransformException& e)
{
ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame);
return false;
}
}
return true;
}
void Node::laser_1_cb(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if(!listener_.waitForTransform("/laser1", "/world", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0))) {
return;
}
sensor_msgs::PointCloud2 cloud;
projector_.transformLaserScanToPointCloud("/world", *scan_in, cloud, listener_);
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROSMsg(cloud, pcl_cloud);
crop_cloud(pcl_cloud, 1, scan_in->header.stamp);
if (!starting_config && fabs(angle) < 0.1)
{
starting_config = 1;
ROS_INFO("Ready to Scan.");
}
if (starting_config)
cloud1 += pcl_cloud;
sensor_msgs::PointCloud2 cloud_out;
pcl::toROSMsg(cloud1, cloud_out);
cloud_out.header = cloud.header;
pc_1_pub_.publish(cloud_out);
}
void SpencerRobotReader::setRobotJointLocation(tf::TransformListener &listener, Joint* joint) {
tf::StampedTransform transform;
std::string jointId = "/";
std::vector<double> jointOrientation;
bg::model::point<double, 3, bg::cs::cartesian> jointPosition;
jointId.append(joint->getName());
try {
ros::Time now = ros::Time::now();
listener.waitForTransform("/map", jointId,
now, ros::Duration(3.0));
listener.lookupTransform("/map", jointId,
now, transform);
//Joint position
jointPosition.set<0>(transform.getOrigin().x());
jointPosition.set<1>(transform.getOrigin().y());
jointPosition.set<2>(transform.getOrigin().z());
//Joint orientation
//curRobot->orientation.push_back(tf::getRoll(transform.getRotation()));
//curRobot->orientation.push_back(tf::getPitch(transform.getRotation()));
jointOrientation.push_back(0.0);
jointOrientation.push_back(0.0);
jointOrientation.push_back(tf::getYaw(transform.getRotation()));
joint->setTime(now.toNSec());
joint->setPosition(jointPosition);
joint->setOrientation(jointOrientation);
} catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
}
void LaserscanVirtualizer::virtual_laser_scan_parser()
{
// LaserScan frames to use for virtualization
istringstream iss(virtual_laser_scan);
vector<string> tokens;
copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));
vector<string> tmp_output_frames;
for(int i=0;i<tokens.size();++i)
{
ros::Time beg = ros::Time::now();
if(tfListener_.waitForTransform (base_frame, tokens[i] ,ros::Time(0),ros::Duration(1.0))) // Check if TF knows the transform from this frame reference to base_frame reference
{
cout << "Elapsed: " << ros::Time::now() - beg << endl;
cout << "Adding: " << tokens[i] << endl;
tmp_output_frames.push_back(tokens[i]);
}
else
cout << "Can't transform: '" << tokens[i] + "' to '" << base_frame << "'" << endl;
}
// Sort and remove duplicates
sort(tmp_output_frames.begin(),tmp_output_frames.end());
std::vector<string>::iterator last = std::unique(tmp_output_frames.begin(), tmp_output_frames.end());
tmp_output_frames.erase(last, tmp_output_frames.end());
// Do not re-advertize if the topics are the same
if( (tmp_output_frames.size() != output_frames.size()) || !equal(tmp_output_frames.begin(),tmp_output_frames.end(),output_frames.begin()) )
{
// Shutdown previous publishers
for(int i=0; i<virtual_scan_publishers.size(); ++i)
virtual_scan_publishers[i].shutdown();
cloud_frame = "";
output_frames = tmp_output_frames;
if(output_frames.size() > 0)
{
virtual_scan_publishers.resize(output_frames.size());
ROS_INFO("Publishing: %ld virtual scans", virtual_scan_publishers.size() );
cout << "Advertising topics: " << endl;
for(int i=0; i<output_frames.size(); ++i)
{
if (output_laser_topic.empty())
{
virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan> (output_frames[i].c_str(), 1);
cout << "\t\t" << output_frames[i] << " on topic " << output_frames[i].c_str() << endl;
}
else
{
virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan> (output_laser_topic.c_str(), 1);
cout << "\t\t" << output_frames[i] << " on topic " << output_laser_topic.c_str() << endl;
}
}
}
else
ROS_INFO("Not publishing to any topic.");
}
}
void commandForce(const geometry_msgs::WrenchStamped::ConstPtr &msg)
{
F_des_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z,
msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
if(msg->header.frame_id == root_name_)
return;
geometry_msgs::PoseStamped in_root;
in_root.pose.orientation.w = 1.0;
in_root.header.frame_id = msg->header.frame_id;
try {
tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
tf_.transformPose(root_name_, in_root, in_root);
}
catch (const tf::TransformException &ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
return;
}
Eigen::Affine3d t;
tf::poseMsgToEigen(in_root.pose, t);
F_des_.head<3>() = t.linear() * F_des_.head<3>();
F_des_.tail<3>() = t.linear() * F_des_.tail<3>();
}
static int getHumanLocation(tf::TransformListener& listener){
tf::StampedTransform transform;
try{
ros::Time now = ros::Time::now();
listener.waitForTransform("/map", "/human_base",
now, ros::Duration(3.0));
listener.lookupTransform("/map", "/human_base",
now, transform);
humanConf.dof[0] = transform.getOrigin().x();
humanConf.dof[1] = transform.getOrigin().y();
humanConf.dof[2] = transform.getOrigin().z()+1.0;
humanConf.dof[3] = 0.0;
humanConf.dof[4] = 0.0;
humanConf.dof[5] = tf::getYaw(transform.getRotation());
if(humanPose_DEBUG){
printf("%f %f %f %f %f %f\n", transform.getOrigin().x(), transform.getOrigin().y(),
transform.getOrigin().z(), humanConf.dof[3], humanConf.dof[4], humanConf.dof[5]);
}
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
return TRUE;
}
void receiveInformation(const geometry_msgs::PointStampedConstPtr &person)
{
notReceivedNumber = 0;
hold = false;
try
{
listener.waitForTransform(robot_frame, person->header.stamp, person->header.frame_id, person->header.stamp, fixed_frame_id, ros::Duration(10.0) );
listener.transformPoint(robot_frame, person->header.stamp, *person, fixed_frame_id, personInRobotBaseFrame);
cv::Point3d personPoint;
personPoint.x = personInRobotBaseFrame.point.x;
personPoint.y = personInRobotBaseFrame.point.y;
personPoint.z = personInRobotBaseFrame.point.z;
distanceToPerson = cv::norm(personPoint);
ROS_ERROR("Person distance: %f", distanceToPerson);
}
catch(tf::TransformException ex)
{
ROS_WARN("%s",ex.what());
//ros::Duration(1.0).sleep();
return;
}
personInRobotBaseFrame.header.frame_id = robot_frame;
}
void Explore::transfromRobotToOdomPosition(float x_robot, float y_robot, float &x_odom, float &y_odom){
// ROS_INFO(" enter transfromRobotToOdomPosition robot pose (%f, %f)", x_robot, y_robot);
ros::Time now = ros::Time::now();
// ROS_INFO("1");
tf::StampedTransform tfRO;
// ROS_INFO("2");
tf_listener_.waitForTransform("/base_link", "/odom", now, ros::Duration(1.0));
// ROS_INFO("3");
tf_listener_.lookupTransform ("/base_link", "/odom", now, tfRO);
// ROS_INFO("4");
tf::Transform tfOR = tfRO.inverse();
// ROS_INFO("5");
float noused = 0.0;
// ROS_INFO("6");
tf::Transform tfRD;
// ROS_INFO("7");
tfRD.setOrigin(tf::Vector3(x_robot, y_robot, noused));
// ROS_INFO("8");
tf::Transform tfOD = tfOR * tfRD;
x_odom = tfOD.getOrigin ()[0]; // wspolrzedna x_robot w ukladzie odom
y_odom = tfOD.getOrigin ()[1]; // wspolrzedna y_robot w ukladzie odom
// ROS_INFO("x_odom = %f, y_odom = %f", x_odom, y_odom);
}
void TransformPose::publish_all(tf::TransformListener& listener)
{
geometry_msgs::PoseStamped odom_pose;
odom_pose.header.frame_id = "/base_link";
//we'll just use the most recent transform available for our simple example
odom_pose.header.stamp = ros::Time();
//just an arbitrary point in space
odom_pose.pose.position.x = 0.0;
odom_pose.pose.position.y = 0.0;
odom_pose.pose.position.z = 0.0;
odom_pose.pose.orientation.x = 0.0;
odom_pose.pose.orientation.y = 0.0;
odom_pose.pose.orientation.z = 0.0;
odom_pose.pose.orientation.w = 1.0;
try{
ros::Time now = ros::Time::now();
listener.waitForTransform("/odom", "/base_link", now, ros::Duration(5.0));
geometry_msgs::PoseStamped base_pose;
listener.transformPose("/odom", odom_pose, base_pose);
my_odom.header.stamp = base_pose.header.stamp;
my_odom.pose.pose.position.x = base_pose.pose.position.x;
my_odom.pose.pose.position.y = base_pose.pose.position.y;
my_odom.pose.pose.position.z = base_pose.pose.position.z;
my_odom.pose.pose.orientation = base_pose.pose.orientation;
//my_maximus_odom.twist.twist.linear.x = sqrt(pow(base_pose.pose.position.x - old_x, 2) + pow(base_pose.pose.position.y - old_y, 2)) / (my_maximus_odom.header.stamp.toSec() - old_time.toSec());
my_odom.twist.twist.linear.x = xspeed.data;
my_odom.twist.twist.linear.y = 0;
my_odom.twist.twist.linear.z = 0;
my_odom.twist.twist.angular.x = 0;
my_odom.twist.twist.angular.y = 0;
my_odom.twist.twist.angular.z = tspeed.data;
//my_maximus_odom.twist.twist.angular.z = 1.1 * (tf::getYaw (my_maximus_odom.pose.pose.orientation) - old_theta) / (my_maximus_odom.header.stamp.toSec() - old_time.toSec());
odom_pub.publish(my_odom);
//old_x = base_pose.pose.position.x;
//old_y = base_pose.pose.position.y;
//old_theta = tf::getYaw (my_odom.pose.pose.orientation);
//old_time = my_odom.header.stamp;
/*
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
my_maximus_odom.pose.pose.position.x, my_maximus_odom.pose.pose.position.y, my_maximus_odom.pose.pose.position.z,
my_maximus_odom.twist.twist.linear.x, my_maximus_odom.twist.twist.linear.y, my_maximus_odom.twist.twist.angular.z, base_pose.header.stamp.toSec());
*/
}
catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"odom\" to \"base_link\": %s", ex.what());
}
}
void init(){
pose_pub_ = node_.advertise<geometry_msgs::PoseStamped>("r_cart/command_pose", 1);
tf::StampedTransform transform;
bool pass = false;
int times_looping = 0;
while (!pass) {
pass = true;
times_looping++;
std::cout << "Times looping " << times_looping << std::endl;
try{
listener.waitForTransform("/torso_lift_link", "/EEframe", ros::Time(0), ros::Duration(5.0));
listener.lookupTransform( "/torso_lift_link", "/EEframe", ros::Time(0), transform);
}
catch (tf::TransformException ex){
pass = false;
ROS_ERROR("%s",ex.what());
}
}
pass = false;
times_looping = 0;
while (!pass) {
pass = true;
times_looping++;
std::cout << "Times looping " << times_looping << std::endl;
try{
listener.waitForTransform("/torso_lift_link", "/des_EEframe", ros::Time(0), ros::Duration(5.0));
listener.lookupTransform( "/torso_lift_link", "/des_EEframe", ros::Time(0), transform);
}
catch (tf::TransformException ex){
pass = false;
ROS_ERROR("%s",ex.what());
}
}
// Load parameters
cmd.header.frame_id = "/torso_lift_link";
cmd.pose.position.x = transform.getOrigin().x();
cmd.pose.position.y = transform.getOrigin().y();
cmd.pose.position.z = transform.getOrigin().z();
cmd.pose.orientation.x = transform.getRotation().x();
cmd.pose.orientation.y = transform.getRotation().y();
cmd.pose.orientation.z = transform.getRotation().z();
cmd.pose.orientation.w = transform.getRotation().w();
}
void pc_callback(const sensor_msgs::PointCloud2ConstPtr msg) {
pcl::PointCloud<pcl::PointXYZ> temp;
pcl::fromROSMsg(*msg, temp);
// Make sure the point cloud is in the base-frame
listener_.waitForTransform(base_frame_,msg->header.frame_id,msg->header.stamp,ros::Duration(1.0));
pcl_ros::transformPointCloud(base_frame_,msg->header.stamp, temp, msg->header.frame_id, lastpc_, listener_);
// unsigned int n = lastpc.size();
// ROS_INFO("New point cloud: %d points",n);
// for (unsigned int i=0;i<n;i++) {
// float x = lastpc[i].x;
// float y = lastpc[i].y;
// float z = lastpc[i].z;
// ROS_INFO("%d %.3f %.3f %.3f",i,x,y,z);
// }
// printf("\n\n\n");
og_ = FREE; // reset the map
unsigned int n = lastpc_.size();
for (unsigned int i=0;i<n;i++) {
float x = lastpc_[i].x;
float y = lastpc_[i].y;
float d = hypot(x,y);
if (d < 1e-2) {
// Bogus point, ignore
continue;
}
cv::Point P(x / map_resolution_ + grid_width_/2., y/map_resolution_ + grid_width_/2.);
cv::circle(og_,P, ceil(robot_radius_/map_resolution_ + 1), cv::Scalar(OCCUPIED), -1); // filled circle
}
// Marked the robot own space as FREE, whatever the point cloud
// is saying. This means that rotation on the spot are always
// possible
cv::Point myself(grid_width_/2,grid_width_/2);
cv::circle(og_,myself, ceil(robot_radius_/map_resolution_ + 1), cv::Scalar(FREE), -1); // filled circle
cv::imshow( "OccGrid", og_ );
//
// First convert the obstacle into the ego-kinematic space
// (d,alpha), where d is the distance on the arc of circle, and
// alpha = atan2(v,w)
cv::remap(og_,d_alpha_,dalpha_remap_x,dalpha_remap_y, CV_INTER_LINEAR, cv::BORDER_CONSTANT, UNKNOWN );
// Now we need to go through all the d_alpha combination. If we hit
// an obstacle at a given value of d, any further distance is also
// going to hit it.
for (unsigned int i=0;i<n_alpha_;i++) {
for (int j=n_d_/2;j<(signed)n_d_;j++) {
if (d_alpha_(j,i) != FREE) {
for (;j<(signed)n_d_;j++) d_alpha_(j,i) = OCCUPIED;
}
}
for (int j=n_d_/2;j>=0;j--) {
if (d_alpha_(j,i) != FREE) {
for (;j>=0;j--) d_alpha_(j,i) = OCCUPIED;
}
}
}
cv::imshow( "DAlpha", d_alpha_ );
}
void wrenchCallback(geometry_msgs::WrenchStamped::ConstPtr wrench_stamped)
{
double cur_time = wrench_stamped->header.stamp.toSec();
if(start_time == -1)
start_time = cur_time;
CartVec w;
wrenchMsgToEigen(wrench_stamped->wrench, w);
double force_mag = NORM(w(0, 0), w(1, 0), w(2, 0));
tf::StampedTransform tool_loc_tf;
try {
tf_list.waitForTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, ros::Duration(0.1));
tf_list.lookupTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, tool_loc_tf);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
return;
}
tool_loc_tf.mult(registration_tf, tool_loc_tf);
btVector3 tool_loc = tool_loc_tf.getOrigin();
PRGB query_pt;
query_pt.x = tool_loc.x(); query_pt.y = tool_loc.y(); query_pt.z = tool_loc.z();
vector<int> nk_inds(1);
vector<float> nk_dists(1);
kd_tree->nearestKSearch(query_pt, 1, nk_inds, nk_dists);
int closest_ind = nk_inds[0];
float closest_dist = nk_dists[0];
hrl_phri_2011::ForceProcessed fp;
fp.time_offset = cur_time - start_time;
tf::transformStampedTFToMsg(tool_loc_tf, fp.tool_frame);
fp.header = wrench_stamped->header;
fp.header.frame_id = "/head_center";
fp.wrench = wrench_stamped->wrench;
fp.pc_ind = closest_ind;
fp.pc_dist = closest_dist;
fp.pc_pt.x = pc_head->points[closest_ind].x;
fp.pc_pt.y = pc_head->points[closest_ind].y;
fp.pc_pt.z = pc_head->points[closest_ind].z;
fp.force_magnitude = force_mag;
if(compute_norms) {
fp.pc_normal.x = normals->points[closest_ind].normal[0];
fp.pc_normal.y = normals->points[closest_ind].normal[1];
fp.pc_normal.z = normals->points[closest_ind].normal[2];
}
// do ellipsoidal processing
tf::Transform tool_loc_ell = ell_reg_tf * tool_loc_tf;
tf::transformTFToMsg(tool_loc_ell, fp.tool_ell_frame);
btVector3 tloce_pos = tool_loc_ell.getOrigin();
ell.cartToEllipsoidal(tloce_pos.x(), tloce_pos.y(), tloce_pos.z(),
fp.ell_coords.x, fp.ell_coords.y, fp.ell_coords.z);
fp_list.push_back(fp);
pub_ind++;
if(pub_ind % 100 == 0)
ROS_INFO("Recorded %d samples", pub_ind);
}
float GoToSelectedBall::getRobotAngleInOdom(){
ros::Time now = ros::Time(0);
tf::StampedTransform tfOR; // robot w odom
tf_listener_.waitForTransform("/odom", "/base_link", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/odom", "/base_link", now, tfOR);
return tf::getYaw(tfOR.getRotation());
}
//Close the gripper
void close(){
ROS_INFO("Closing Gripper");
ros::Duration(0.5).sleep();
//wait for the listener to get the first message
listener_.waitForTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), ros::Duration(1.0));
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
//record the starting transform from the gripper to the base frame
listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), start_transform);
bool done = false;
pr2_controllers_msgs::Pr2GripperCommand gripper_cmd;
gripper_cmd.position = 0.0;
gripper_cmd.max_effort = 50.0;
ros::Rate rate(10.0);
double dist_moved_before;
double dist_moved;
while (!done && nh_.ok())
{
r_gripper_.publish(gripper_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
//see how if the gripper is open or if it hit some object
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
dist_moved_before = dist_moved;
dist_moved = relative_transform.getOrigin().length();
//ROS_INFO("%f",dist_moved);
if(dist_moved > 0.03 || dist_moved < dist_moved_before) done = true;
}
}
void Explore::getRobotPositionInOdom(float &x_odom_pose, float &y_odom_pose){
ros::Time now = ros::Time(0);
tf::StampedTransform tfOR; // robot w odom
tf_listener_.waitForTransform("/odom", "/base_link", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/odom", "/base_link", now, tfOR);
x_odom_pose = tfOR.getOrigin ()[0]; // wspolrzedne robota w ukladzie odom
y_odom_pose = tfOR.getOrigin ()[1]; // wspolrzedne robota w ukladzie odom
}
void callback(const sensor_msgs::ImageConstPtr& msg) {
// First extract the image to a cv:Mat structure, from opencv2
cv::Mat img(cv_bridge::toCvShare(msg,"mono8")->image);
try{
// Then receive the transformation between the robot body and
// the world
tf::StampedTransform transform;
// Use the listener to find where we are. Check the
// tutorials... (note: the arguments of the 2 functions below
// need to be completed
listener_.waitForTransform("","",ros::Time::now(),ros::Duration(1.0));
listener_.lookupTransform("","", ros::Time::now(), transform);
double proj_x = transform.getOrigin().x();
double proj_y = transform.getOrigin().y();
double proj_theta = -tf::getYaw(transform.getRotation());
printf("We were at %.2f %.2f theta %.2f\n",proj_x,proj_y,proj_theta*180./M_PI);
// Once the transformation is know, you can use it to find the
// affine transform mapping the local floor to the global floor
// and use cv::warpAffine to fill p_floor
cv::Mat_<uint8_t> p_floor(floor_size_pix,floor_size_pix,0xFF);
cv::Mat affine = (cv::Mat_<float>(2,3)
<< 1, 0, 0,
0, 1, 0);
cv::warpAffine(img,p_floor,affine, p_floor.size(),
cv::INTER_NEAREST,cv::BORDER_CONSTANT,0xFF);
// This published the projected floor on the p_floor topic
cv_bridge::CvImage pbr(msg->header,"mono8",p_floor);
ptpub_.publish(pbr.toImageMsg());
// Now that p_floor and floor have the same size, you can use
// the following for loop to go through all the pixels and fuse
// the current observation with the previous one.
for (unsigned int j=0;j<(unsigned)floor_.rows;j++) {
for (unsigned int i=0;i<(unsigned)floor_.cols;i++) {
uint8_t p = p_floor.at<uint8_t>(i,j);
uint8_t f = floor_.at<uint8_t>(i,j);
// Compute the new value of pixel i,j here
floor_.at<uint8_t>(i,j) = f;
}
}
// Finally publish the floor estimation.
cv_bridge::CvImage br(msg->header,"mono8",floor_);
tpub_.publish(br.toImageMsg());
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
tf::StampedTransform TargetPointSender::getRobotPosGL()
{
tf::StampedTransform robot_gl;
try{
tf_listener_.waitForTransform(world_frame_, robot_frame_, ros::Time(0.0), ros::Duration(4.0));
tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
}catch(tf::TransformException &e){
ROS_WARN_STREAM("tf::TransformException: " << e.what());
}
return robot_gl;
}
float Explore::getRobotAngleInOdom(){
ros::Time now = ros::Time(0);
tf::StampedTransform tfOR; // robot w odom
tf_listener_.waitForTransform("/odom", "/base_link", now, ros::Duration(1.0));
tf_listener_.lookupTransform ("/odom", "/base_link", now, tfOR);
// x_odom_pose = tfOR.getOrigin ()[0]; // wspolrzedne robota w ukladzie odom
// y_odom_pose = tfOR.getOrigin ()[1]; // wspolrzedne robota w ukladzie odom
return tf::getYaw(tfOR.getRotation());
}
void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
{
if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
{
try
{
// update the pose
listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);
listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);
tf::Vector3 position_( kinect2map.getOrigin() );
position.x() = position_.x();
position.y() = position_.y();
position.z() = position_.z();
tf::Quaternion orientation_( kinect2map.getRotation() );
orientation.x() = orientation_.x();
orientation.y() = orientation_.y();
orientation.z() = orientation_.z();
orientation.w() = orientation_.w();
ROS_INFO_STREAM("position = " << position.transpose() );
ROS_INFO_STREAM("orientation = " << orientation.transpose() );
// update the cloud
pcl::copyPointCloud(*cloud, *xyz_cld_ptr); // Do I need to copy it?
//xyz_cld_ptr = cloud;
cloud_updated = true;
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
}
}
CartController(ros::NodeHandle &nh, std::string controller_name, std::string position_controller_name, std::string lwr_side) :
TestController(nh, controller_name, position_controller_name, lwr_side)
{
initialized_ = false;
ROS_INFO_STREAM("Subscribing to: " << topic_joint_states_);
sub_js_ = nh_.subscribe (topic_joint_states_, 10, &CartController::callbackJointState, this);
//ROS_INFO_STREAM ("Subscribing to /lwr/" << controller_name_ << "/measured_cartesian_position...");
//sub_pose_ = nh_.subscribe ("/lwr/" + controller_name_ + "/measured_cartesian_pose", 10, &CartController::callbackCartPosition, this);
ROS_INFO_STREAM ("Waiting for base_link Transform...");
try {
//ros::Time ros_time = ros::Time(0);
tf_listener_.waitForTransform(base_link_, lwr_tip_link_, ros::Time(0), ros::Duration(5.0));
tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
getCartBaseLinkPosition();
ROS_INFO_STREAM ("done!");
std::string topic_pose = lwr_ns_ + "/" + controller_name_ + "/pose";
std::string topic_pose_base_link = lwr_ns_ + "/" + controller_name_ + "/pose_base_link";
std::string topic_addFT = lwr_ns_ + "/" + controller_name_ + "/addFT";
std::string topic_gains = lwr_ns_ + "/" + controller_name_ + "/gains";
ROS_INFO_STREAM("Creating publishers to: " << endl <<
topic_pose << endl <<
topic_pose_base_link << endl <<
topic_addFT << endl <<
topic_gains);
pub_pose_ = nh_.advertise<geometry_msgs::Pose>(topic_pose, 500);
pub_pose_base_link_ = nh_.advertise<geometry_msgs::Pose>(topic_pose_base_link, 500);
pub_force_ = nh_.advertise<std_msgs::Float64MultiArray> (topic_addFT, 500);
pub_gains_ = nh_.advertise<std_msgs::Float64MultiArray> (topic_gains, 500);
ROS_INFO("Creating service handle: switch_controller");
srv_contr_switcher_ = nh_.serviceClient<controller_manager_msgs::SwitchController>(lwr_ns_ + "/controller_manager/switch_controller");
ROS_INFO("Creating service handle: list_controllers");
srv_contr_list_ = nh_.serviceClient<controller_manager_msgs::ListControllers>(lwr_ns_ + "/controller_manager/list_controllers");
// if required controller not running, switch controller
if (!checkController()) {
switchController(controller_name, position_controller_name);
}
}
bool getGoalPose (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
if (global_plan.empty())
{
ROS_ERROR ("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try
{
tf::StampedTransform transform;
tf.waitForTransform (global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration (0.5));
tf.lookupTransform (global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF (plan_goal_pose, goal_pose);
goal_pose.setData (transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch (tf::LookupException& ex)
{
ROS_ERROR ("No Transform available Error: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR ("Connectivity Error: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR ("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
btTransform waitForAndGetTransform(const tf::TransformListener& listener, std::string target_frame, std::string source_frame) {
tf::StampedTransform st;
while(ros::ok()) {
try {
listener.waitForTransform(target_frame, source_frame, ros::Time(0),ros::Duration(.1));
listener.lookupTransform(target_frame, source_frame, ros::Time(0), st);
} catch (...) {
ROS_WARN("An exception was catched from waitForAndGetTransform. Retrying...");
continue;
}
break;
}
return st.asBt();
}
