void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
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);
}
void InputOutputLinControl::updateMapRobotPose(tf::Transform relative_transform, tf::StampedTransform current_icp_robot_pose, tf::StampedTransform& next_icp_robot_pose)
{
tf::Transform transformation = relative_transform * current_icp_robot_pose;
next_icp_robot_pose.frame_id_ = current_icp_robot_pose.frame_id_;
next_icp_robot_pose.stamp_ = ros::Time::now();
next_icp_robot_pose.child_frame_id_ = current_icp_robot_pose.child_frame_id_;
next_icp_robot_pose.setBasis(transformation.getBasis());
next_icp_robot_pose.setOrigin(transformation.getOrigin());
next_icp_robot_pose.setRotation(transformation.getRotation());
}
void servo_callback(const std_msgs::Float32::ConstPtr& angle_msg)
{
ROS_INFO("angle: %f", angle_msg->data);
tf::Quaternion quat = tf::createQuaternionFromRPY(((angle_msg->data+90.0)/180.0)*M_PI, 0.0, 0.0);
trans.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
trans.frame_id_ = odom;
trans.child_frame_id_ = base_link;
trans.setRotation(quat);
trans.stamp_ = ros::Time::now(); //probably not valid, need to stamp the angle on the arduino itself
}
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_);
}
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::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());
}
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);
}
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;
}
// Load full content of a given directory
bool TRPBViz::loadDirectory(const string& directory) {
PointCloud tmp_cloud;
// Input point cloud
sensor_msgs::PointCloud2 input_point_cloud;
bool ipc = deSerializePointCloud(tmp_cloud,
directory+"/input_point_cloud.csv");
if (ipc) {
input_point_cloud = PointMatcher_ros::\
pointMatcherCloudToRosMsg<float>(tmp_cloud, "/map",
ros::Time::now());
input_point_cloud_pub.publish(input_point_cloud);
}
// Sparse map point cloud (if any)
sensor_msgs::PointCloud2 sparse_map;
bool sm = deSerializePointCloud(tmp_cloud,
directory+"/sparse_map.csv");
if (sm) {
sparse_map = PointMatcher_ros::\
pointMatcherCloudToRosMsg<float>(tmp_cloud, "/map",
ros::Time::now());
sparse_map_pub.publish(sparse_map);
}
// Robot position (start)
geometry_msgs::Pose start;
// Goal position
geometry_msgs::Pose goal;
bool sg = loadStartGoal(directory+"/start_goal.csv", start, goal);
if (sg) {
// FIXME correct z by 0.0705
tf::poseMsgToTF(start, tf_start);
tf_start.frame_id_ = "/map";
tf_start.child_frame_id_ = "/base_footprint";
tf_start.stamp_ = ros::Time::now();
tf_broadcaster.sendTransform(tf_start);
visualization_msgs::Marker marker;
marker.header.frame_id = "/map";
marker.header.stamp = ros::Time();
marker.ns = "Goal";
marker.id = 0;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = goal;
marker.scale.x = 0.60;
marker.scale.y = 0.60;
marker.scale.z = 0.5;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
marker.color.a = .75;
marker.lifetime = ros::Duration();
goal_pub.publish(marker);
} else {
tf_start.setOrigin({0, 0, 0});
tf_start.setRotation({0, 0, 0, 1});
tf_start.frame_id_ = "/map";
tf_start.child_frame_id_ = "/base_footprint";
}
// Path
nav_msgs::Path path;
bool p = loadPath(directory+"/path.csv", path);
if (p) {
ref_pub.publish(path);
}
return (ipc||sm||sg||p);
}