bool poseTracker::newPose(tf::StampedTransform transform){
Eigen::Vector3f curr(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
pose.push(curr);
if(pose.size() < 50){
return true;
}
float squaredNorm = (pose.front() - curr).squaredNorm();
pose.pop();
bool tmp = squaredNorm > 0.2;
std::cout << tmp << " Max: " << squaredNorm << std::endl;
return squaredNorm > 0.2;
}
// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);
// setup init map
bool init_set_map[MAP_CELLS];
for (int i=0; i<MAP_CELLS;i++) {
init_set_map[i] = false;
}
setVal(init_set_map,goal_pt.x,goal_pt.y,true);
//updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = pointCloud.points.size();
dataContainer.clear();
tf::Vector3 laserPos (laserTransform.getOrigin());
dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
for (size_t i = 0; i < size; ++i)
{
const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
continue;
}
tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
{
dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
}
}
}
return true;
}
carmen_6d_point
get_carmen_pose6D_from_matrix(Matrix visual_odometry_transformation_matrix)
{
// // See: http://www.ros.org/wiki/tf
tf::Transform visual_odometry_pose;
tf::Transform carmen_pose;
carmen_6d_point carmen_pose_6d;
// convert the visual odometry output matrix to the tf::Transform type.
visual_odometry_pose = convert_matrix_to_tf_transform(visual_odometry_transformation_matrix);
// compute the current visual odometry pose with respect to the carmen coordinate system
carmen_pose = g_car_to_visual_odometry_transform * visual_odometry_pose * g_car_to_visual_odometry_transform.inverse();
double yaw, pitch, roll;
tf::Matrix3x3(carmen_pose.getRotation()).getRPY(roll, pitch, yaw);
carmen_pose_6d.x = carmen_pose.getOrigin().x();
carmen_pose_6d.y = carmen_pose.getOrigin().y();
carmen_pose_6d.z = carmen_pose.getOrigin().z();
carmen_pose_6d.yaw = yaw;
carmen_pose_6d.pitch = pitch;
carmen_pose_6d.roll = roll;
return carmen_pose_6d;
}
// wait for one camerainfo, then shut down that subscriber
void cam_info_callback(const sensor_msgs::CameraInfo &msg)
{
camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages);
// handle cartesian offset between stereo pairs
// see the sensor_msgs/CamaraInfo documentation for details
rightToLeft.setIdentity();
rightToLeft.setOrigin(
tf::Vector3(
-msg.P[3]/msg.P[0],
-msg.P[7]/msg.P[5],
0.0));
cam_info_received = true;
cam_info_sub.shutdown();
}
bool spawnObject(){
try{
ros::Time time = ros::Time::now();
tf_listener.waitForTransform("table_top", "object", time, ros::Duration(2.0));
tf_listener.lookupTransform("table_top", "object", time, transform);
}catch(tf::TransformException ex){
return false;
}
objectHeight = transform.getOrigin().getZ();
moveit_msgs::ApplyPlanningScene srv;
moveit_msgs::PlanningScene planning_scene;
planning_scene.is_diff = true;
moveit_msgs::CollisionObject object;
object.header.frame_id = "object";
object.id = "can";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.push_back(objectHeight); //height
primitive.dimensions.push_back(0.04); //radius
object.primitives.push_back(primitive);
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.z = -objectHeight/2;
object.primitive_poses.push_back(pose);
place_pose.header.frame_id = "table_top";
place_pose.pose.orientation.x = 0.5;
place_pose.pose.orientation.y = 0.5;
place_pose.pose.orientation.z = -0.5;
place_pose.pose.orientation.w = 0.5;
place_pose.pose.position.x = transform.getOrigin().getX();
place_pose.pose.position.y = transform.getOrigin().getY();
place_pose.pose.position.z = transform.getOrigin().getZ() + 0.005;
object.operation = object.ADD;
planning_scene.world.collision_objects.push_back(object);
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);
return true;
}
/**
* Convert tf::StampedTransform to string
*/
template<> std::string toString<tf::StampedTransform>(const tf::StampedTransform& p_transform)
{
std::stringstream ss;
ss << "StampedTransform(Quaternion=" << toString(p_transform.getRotation()) << "; Vector3=" << toString(p_transform.getOrigin()) << ")";
return ss.str();
}
void poseStampedToStampedTF(geometry_msgs::PoseStamped &msg, tf::StampedTransform &stampedTF, std::string child)
{
tf::Transform btTrans;
stampedTF.stamp_ = msg.header.stamp;
stampedTF.frame_id_ = msg.header.frame_id;
stampedTF.child_frame_id_ = child;
tf::poseMsgToTF(msg.pose, btTrans);
stampedTF.setData(btTrans);
}
void StigmergyPlanner::makeViewFacingMarker(tf::StampedTransform robot_pose)
{
int_marker.header.frame_id = robot_pose.frame_id_;
int_marker.header.stamp = robot_pose.stamp_;
int_marker.pose.position.x = robot_pose.getOrigin().getX();
int_marker.pose.position.y = robot_pose.getOrigin().getY();
int_marker.pose.position.z = robot_pose.getOrigin().getZ();
int_marker.scale = 1;
int_marker.name = "view_facing";
int_marker.description = "3D Planning pencil";
InteractiveMarkerControl control;
// make a control that rotates around the view axis
//control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
//control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
//control.orientation.w = 1;
//control.name = "rotate";
//int_marker.controls.push_back(control);
// create a box in the center which should not be view facing,
// but move in the camera plane.
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
control.name = "move";
control.markers.push_back( makeBox(int_marker) );
control.always_visible = true;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MENU;
control.name = "menu";
int_marker.controls.push_back(control);
server->insert(int_marker);
server->setCallback(int_marker.name, boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.apply(*server, int_marker.name);
}
inline bool checkTF(tf::StampedTransform& transform, geometry_msgs::PoseStamped& robo,
tf::TransformListener& listener)
{
try{
//listener.waitForTransform(header_frame, robot_frame, ros::Time(0), ros::Duration(1.0));
listener.lookupTransform(header_frame, robot_frame, ros::Time(0), transform);
//copy robot position
robo.pose.position.x=transform.getOrigin().x();
robo.pose.position.y=transform.getOrigin().y();
robo.pose.position.z=0;
float angle = tf::getYaw(transform.getRotation());
robo.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,angle);
return true;
}
catch (tf::TransformException ex){
// cout<<"waiting for global and robot frame relation"<<endl;
ROS_ERROR("%s",ex.what());
return false;
}
}
void sptam::sptam_node::fixOdomFrame(const CameraPose& cameraPose, const tf::StampedTransform& camera_to_odom, const ros::Time& t)
{
tf::Pose camera_to_map;
CameraPose2TFPose( cameraPose, camera_to_map );
// compute the new difference between map and odom
tf::Transform odom_to_map = camera_to_map * camera_to_odom.inverse();
std::lock_guard<std::mutex> lock( odom_to_map_mutex_ );
odom_to_map_ = odom_to_map;
}
bool
WmObjectCapture::updateObjectFrame(const ros::Time& stamp, tf::StampedTransform& m2c)
{
std::vector<tf::StampedTransform> valids;
getValidMarks(valids, stamp);
if(valids.size()<4)
{
//ROS_INFO("No enough marks detected [%zu]", valids.size());
return false;
}
tf::Transform media;
double stdev;
getBestTransform(valids, media, stdev);
double m_tolerance2 = 0.006*0.006;
if(stdev<m_tolerance2)
{
//ROS_INFO("TRANSFORM ACCEPTED ");
m2c.child_frame_id_ = objectFrameId_;
m2c.frame_id_ = cameraFrameId_;
m2c.stamp_ = ros::Time::now();
m2c.setOrigin(media.getOrigin());
m2c.setRotation(media.getRotation());
ROS_DEBUG("NEW FINAL (%lf, %lf, %lf) (%lf, %lf, %lf, %lf)",
m2c.getOrigin().x(), m2c.getOrigin().y(), m2c.getOrigin().z(),
m2c.getRotation().x(), m2c.getRotation().y(), m2c.getRotation().z(), m2c.getRotation().w());
try
{
tfBroadcaster_.sendTransform(m2c);
}catch(tf::TransformException & ex)
{
ROS_WARN("WmObjectCapture::updateObjectFrame %s",ex.what());
}
return true;
}
//ROS_INFO("TRANSFORM REJECTED ");
return false;
}
void InputOutputLinControl::odomMsgToStampedTransformB(double displacement, double yaw, nav_msgs::Odometry pose_odometry, tf::StampedTransform& pose_stamped)
{
pose_stamped.stamp_ = pose_odometry.header.stamp;
pose_stamped.frame_id_ = pose_odometry.header.frame_id;
pose_stamped.child_frame_id_ = pose_odometry.child_frame_id;
tf::Vector3 v;
v.setX(pose_odometry.pose.pose.position.x + displacement*cos(yaw));
v.setY(pose_odometry.pose.pose.position.y + displacement*sin(yaw));
v.setZ(pose_odometry.pose.pose.position.z);
pose_stamped.setOrigin(v);
tf::Quaternion q;
q.setX(pose_odometry.pose.pose.orientation.x);
q.setY(pose_odometry.pose.pose.orientation.y);
q.setZ(pose_odometry.pose.pose.orientation.z);
q.setW(pose_odometry.pose.pose.orientation.w);
pose_stamped.setRotation(q);
}
void foutAppendTF(tf::StampedTransform &transform, std::ofstream &fout)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
fout<< originVector.x() << " "
<< originVector.y() << " "
<< originVector.z() << " "
<< pcl::rad2deg(roll) << " "
<< pcl::rad2deg(pitch) << " "
<< pcl::rad2deg(yaw) << std::endl;
}
bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{
pose.stamp_ = ros::Time::now();
double roll, pitch, yaw;
pose.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
double theta = yaw + angular_vel * timestep;
//Eulerian integration
v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
v.setZ(pose.getOrigin().getZ());
pose.setOrigin(v);
pose.setRotation(tf::createQuaternionFromYaw(theta));
ROS_INFO("New orientation [%f]",theta);
return true;
}
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());
}
}
}
void printTF(tf::StampedTransform &transform, std::string str)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
ROS_INFO("TF %s (x y z r p y): %f %f %f %f %f %f",
str.c_str(),
originVector.x(),
originVector.y(),
originVector.z(),
pcl::rad2deg(roll),
pcl::rad2deg(pitch),
pcl::rad2deg(yaw));
}
inline Localization(vector<ObstacleC> *obstacles) :
location(-1.42, 0), locationKalman(location), kalmanI(
locationKalman), _cameraProjections(
NULL), lastOdomX(0), lastOdomY(0), globalPos(0, 0, 0), lastOdom(0,
0, 0), lastOdomID(0), ballPos(0, 0), m_field(
field_model::FieldModel::getInstance()), obstacles(
obstacles), CurrentVertexId(0), PreviousVertexId(0), LandmarkCount(
0), odomLastGet(0, 0), atLeastOneObservation(false), nodeCounter(
0)
{
odom_sub = nodeHandle.subscribe("/gait/odometry", 10,
&Localization::dead_reckoning_callback, this);
setLoc_sub = nodeHandle.subscribe<geometry_msgs::Point>(
"/vision/setLocation", 1, &Localization::setloc_callback, this);
A = m_field->length();
B = m_field->width();
E = m_field->goalAreaDepth();
F = m_field->goalAreaWidth();
G = m_field->penaltyMarkerDist();
H = m_field->centerCircleDiameter();
D = m_field->goalWidth();
I = m_field->boundary();
params.ball->radius.set(m_field->ballDiameter() / 2.);
A2 = A / 2.;
B2 = B / 2.;
H2 = H / 2.;
A2 = A / 2.;
B2 = B / 2.;
E2 = E / 2.;
F2 = F / 2.;
G2 = G / 2.;
H2 = H / 2.;
D2 = D / 2.;
I2 = I / 2.;
// TF transforms
tfLocField.frame_id_ = "/ego_floor";
tfLocField.child_frame_id_ = "/loc_field";
tfLocField.setIdentity();
lastSavedNodeTime = ros::Time::now();
// create the linear solver
linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();
// create the block solver on the top of the linear solver
blockSolver = new BlockSolverX(linearSolver);
//create the algorithm to carry out the optimization
optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);
}
void addRead(string hexID, int rssi, tf::StampedTransform trans) {
if (rssi == -1) //The tags sometimes see each other; don't record this
return;
//Check to see if this tag has been seen yet
if (heatmaps.find(hexID) == heatmaps.end()) {
heatmaps[hexID] = PointCloud();
heatmapPoses[hexID] = PoseArray();
ChannelFloat32 rgbChannel;
rgbChannel.name = "rgb";
heatmaps[hexID].channels.push_back(rgbChannel);
heatmaps[hexID].header.frame_id = "/map";
heatmapSelected[hexID] = true;
}
double x = trans.getOrigin().x();
double y = trans.getOrigin().y();
double z = trans.getOrigin().z();
Point32 P32;
P32.x = x;P32.y = y;P32.z = z;
Point P;
P.x = x;P.y = y;P.z = z;
btQuaternion rotation = trans.getRotation();
Quaternion Q;
Q.x = rotation.x();Q.y = rotation.y();Q.z = rotation.z();Q.w = rotation.w();
Pose pose;
pose.position = P;
pose.orientation = Q;
float float_rgb;
if (rssi > 10) {
double l = ((double)rssi + 50.0) / 200.0;
int rgb = getColorHSL(20.0, 240.0, l, rssi);
float_rgb = *reinterpret_cast<float*>(&rgb);
}
heatmaps[hexID].points.push_back(P32);
//Color of point in RGB channel
heatmaps[hexID].channels[0].values.push_back(float_rgb);
heatmapPoses[hexID].poses.push_back(pose);
}
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
if (fabs(timeOdomBefMapped - timeOdomAftMapped) < 0.005) {
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;
transformAssociateToMap();
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(transformMapped[2], -transformMapped[0], -transformMapped[1]);
laserOdometry2.header.stamp = laserOdometry->header.stamp;
laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
laserOdometry2.pose.pose.orientation.z = geoQuat.x;
laserOdometry2.pose.pose.orientation.w = geoQuat.w;
laserOdometry2.pose.pose.position.x = transformMapped[3];
laserOdometry2.pose.pose.position.y = transformMapped[4];
laserOdometry2.pose.pose.position.z = transformMapped[5];
pubLaserOdometry2Pointer->publish(laserOdometry2);
laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}
}
geometry_msgs::PoseStamped OdomTf::get_pose_from_transform(tf::StampedTransform tf) {
//clumsy conversions--points, vectors and quaternions are different data types in tf vs geometry_msgs
geometry_msgs::PoseStamped stPose;
geometry_msgs::Quaternion quat; //geometry_msgs object for quaternion
tf::Quaternion tfQuat; // tf library object for quaternion
tfQuat = tf.getRotation(); // member fnc to extract the quaternion from a transform
quat.x = tfQuat.x(); // copy the data from tf-style quaternion to geometry_msgs-style quaternion
quat.y = tfQuat.y();
quat.z = tfQuat.z();
quat.w = tfQuat.w();
stPose.pose.orientation = quat; //set the orientation of our PoseStamped object from result
// now do the same for the origin--equivalently, vector from parent to child frame
tf::Vector3 tfVec; //tf-library type
geometry_msgs::Point pt; //equivalent geometry_msgs type
tfVec = tf.getOrigin(); // extract the vector from parent to child from transform
pt.x = tfVec.getX(); //copy the components into geometry_msgs type
pt.y = tfVec.getY();
pt.z = tfVec.getZ();
stPose.pose.position = pt; //and use this compatible type to set the position of the PoseStamped
stPose.header.frame_id = tf.frame_id_; //the pose is expressed w/rt this reference frame
stPose.header.stamp = tf.stamp_; // preserve the time stamp of the original transform
return stPose;
}
int main(int argc, char **argv) {
ros::init(argc, argv, ROS_PACKAGE_NAME);
ros::NodeHandle n;
ros::NodeHandle pn("~");
pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
pn.param("base_frame", p_base_frame_, std::string("base_link"));
tfB_ = new tf::TransformBroadcaster();
transform_.getOrigin().setX(0.0);
transform_.getOrigin().setY(0.0);
transform_.getOrigin().setZ(0.0);
transform_.frame_id_ = p_base_stabilized_frame_;
transform_.child_frame_id_ = p_base_frame_;
ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback);
ros::spin();
delete tfB_;
return 0;
}
void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
{
tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
tfScalar yaw, pitch, roll;
tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
tmp_.setRPY(roll, pitch, 0.0);
transform_.setRotation(tmp_);
transform_.stamp_ = imu_msg.header.stamp;
tfB_->sendTransform(transform_);
}
tf::StampedTransform OdomTf::stamped_transform_inverse(tf::StampedTransform stf) {
// instantiate stamped transform with constructor args
//note: child_frame and frame_id are reversed, to correspond to inverted transform
tf::StampedTransform stf_inv(stf.inverse(), stf.stamp_, stf.child_frame_id_, stf.frame_id_);
/* long-winded equivalent:
tf::StampedTransform stf_inv;
tf::Transform tf = get_tf_from_stamped_tf(stf);
tf::Transform tf_inv = tf.inverse();
stf_inv.stamp_ = stf.stamp_;
stf_inv.frame_id_ = stf.child_frame_id_;
stf_inv.child_frame_id_ = stf.frame_id_;
stf_inv.setOrigin(tf_inv.getOrigin());
stf_inv.setBasis(tf_inv.getBasis());
* */
return stf_inv;
}
void printTF(tf::StampedTransform t, WhichArm a)
{
std::string p;
if(a==RIGHT)
{
std::cout<< "#Right gripper:\n";
p="r";
}
else
{
std::cout<< "#Left gripper:\n";
p="l";
}
std::cout<< "p.position.x = " << t.getOrigin().getX() << ";\n"
<< "p.position.y = " << t.getOrigin().getY() << ";\n"
<< "p.position.z = " << t.getOrigin().getZ() << ";\n"
<< "p.orientation.x = " << t.getRotation().getX() << ";\n"
<< "p.orientation.y = " << t.getRotation().getY() << ";\n"
<< "p.orientation.z = " << t.getRotation().getZ() << ";\n"
<< "p.orientation.w = " << t.getRotation().getW() << ";\n";
geometry_msgs::PoseStamped tmp;
tmp.pose.position.x = t.getOrigin().getX() ;
tmp.pose.position.y = t.getOrigin().getY() ;
tmp.pose.position.z = t.getOrigin().getZ() ;
tmp.pose.orientation.x = t.getRotation().getX();
tmp.pose.orientation.y = t.getRotation().getY();
tmp.pose.orientation.z = t.getRotation().getZ();
tmp.pose.orientation.w = t.getRotation().getW();
std::cout<< " pose:\n"
<< " position:\n"
<< " x: " << tmp.pose.position.x << "\n"
<< " y: " << tmp.pose.position.y << "\n"
<< " z: " << tmp.pose.position.z << "\n"
<< " orientation:\n"
<< " x: " << tmp.pose.orientation.x << "\n"
<< " y: " << tmp.pose.orientation.y << "\n"
<< " z: " << tmp.pose.orientation.z << "\n"
<< " w: " << tmp.pose.orientation.w << "\n\n";
// std::cout<<tmp<<"\n";
}
void getPoseInRobotFrame(std::vector<SM_COND> nextRobotCond_w)
{
//transforming position into robot frame
geometry_msgs::PoseStamped goal;
geometry_msgs::PoseStamped yawMapRobot;
geometry_msgs::PoseStamped goalRobotFrame;
// position
goal.header.stamp= ros::Time(0);
goal.header.frame_id= "map";
goal.pose.position.x= nextRobotCond_w[0].x;
goal.pose.position.y= nextRobotCond_w[1].x;
goal.pose.position.z= 0.0;
tf::Quaternion q = tf::createQuaternionFromRPY(0.0,0.0,nextRobotCond_w[5].x);
tf::quaternionTFToMsg(q,goal.pose.orientation);
_listener.waitForTransform("base_footprint", "map",
ros::Time(0), ros::Duration(1.0));
_listener.transformPose("base_footprint", goal, goalRobotFrame);
_nextRobotCond_r[0].x = goalRobotFrame.pose.position.x;
_nextRobotCond_r[1].x = goalRobotFrame.pose.position.y;
_nextRobotCond_r[5].x = tf::getYaw(goalRobotFrame.pose.orientation);
// velocity and acceleration
//record the starting transform from the odometry to the base frame
_listener.lookupTransform("map", "base_footprint",
ros::Time(0), world_transform);
double roll, pitch, yaw;
q = world_transform.getRotation();
btMatrix3x3(q).getRPY(roll, pitch, yaw);
_nextRobotCond_r[0].v = cos(yaw)*nextRobotCond_w[0].v + sin(yaw)*nextRobotCond_w[1].v;
_nextRobotCond_r[1].v = -sin(yaw)*nextRobotCond_w[0].v + cos(yaw)*nextRobotCond_w[1].v;
_nextRobotCond_r[5].v = nextRobotCond_w[5].v;
_nextRobotCond_r[0].a = cos(yaw)*nextRobotCond_w[0].a + sin(yaw)*nextRobotCond_w[1].a;
_nextRobotCond_r[1].a = -sin(yaw)*nextRobotCond_w[0].a + cos(yaw)*nextRobotCond_w[1].a;
_nextRobotCond_r[5].a = nextRobotCond_w[5].a;
}
void MocapAlign::spinOnce( )
{
static tf::StampedTransform tf_a;
static tf::StampedTransform tf_b;
try
{
li.lookupTransform( frame_base, frame_a, ros::Time(0), tf_a );
li.lookupTransform( frame_base, frame_b, ros::Time(0), tf_b );
}
catch( tf::TransformException ex )
{
ROS_INFO( "Missed a transform...chances are that we are still OK" );
return;
}
if( tf_a.getOrigin( ).x( ) != tf_a.getOrigin( ).x( ) ||
tf_b.getOrigin( ).x( ) != tf_b.getOrigin( ).x( ) )
{
ROS_WARN( "NaN DETECTED" );
return;
}
// Rotate us to be aligned with the uav
const tf::Quaternion delta_quat = tf::createQuaternionFromRPY( 0, 0, tf::getYaw( tf_a.getRotation( ) ) - tf::getYaw( tf_b.getRotation( ) ) );
const tf::Quaternion quat_b_aligned = tf_b.getRotation( ) * delta_quat;
// Broadcast the aligned tf
tf::StampedTransform tf_b_aligned( tf_b );
if( tf_b_aligned.getOrigin( ).x( ) != tf_b_aligned.getOrigin( ).x( ) )
{
ROS_WARN( "NaN PRODUCED" );
return;
}
tf_b_aligned.setRotation( quat_b_aligned );
tf_b_aligned.child_frame_id_ = tf_b_aligned.child_frame_id_ + "_aligned";
br.sendTransform( tf_b_aligned );
}
void ObservationModel::integratePoseMeasurement(Particles& particles, double poseRoll, double posePitch, const tf::StampedTransform& footprintToTorso){
double poseHeight = footprintToTorso.getOrigin().getZ();
ROS_DEBUG("Pose measurement z=%f R=%f P=%f", poseHeight, poseRoll, posePitch);
// TODO cluster xy of particles => speedup
#pragma omp parallel for
for (unsigned i=0; i < particles.size(); ++i){
// integrate IMU meas.:
double roll, pitch, yaw;
particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
particles[i].weight += m_weightRoll * logLikelihood(poseRoll - roll, m_sigmaRoll);
particles[i].weight += m_weightPitch * logLikelihood(posePitch - pitch, m_sigmaPitch);
// integrate height measurement (z)
double heightError = 0;
// if (getHeightError(particles[i],footprintToTorso, heightError))
// particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
}
}
void InputOutputLinControl::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
true_robot_pose.frame_id_ = robot_poseB.frame_id_;
true_robot_pose.stamp_ = robot_poseB.stamp_;
double roll, pitch, yaw;
robot_poseB.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
v.setZ(robot_poseB.getOrigin().getZ());
true_robot_pose.setOrigin(v);
true_robot_pose.setRotation(robot_poseB.getRotation());
}
void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
real_robot_poseB.stamp_ = real_robot_pose.stamp_;
real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;
tf::Vector3 v;
double roll, pitch, yaw;
real_robot_pose.getBasis().getRPY(roll,pitch,yaw);
v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
v.setZ(real_robot_pose.getOrigin().getZ());
real_robot_poseB.setOrigin(v);
real_robot_poseB.setRotation(real_robot_pose.getRotation());
}
