diff --git a/CMakeLists.txt b/CMakeLists.txt
index 52f73c77eb02b43c3f4b96f64db3fec9ce8475ec..b6556e062814bc3de6d130b8363bd44f9a663c9b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,6 +24,7 @@ find_package(Eigen3 REQUIRED)
add_message_files(
FILES
collision.msg
+ relocalization.msg
)
## Generate services in the 'srv' folder
diff --git a/msg/relocalization.msg b/msg/relocalization.msg
new file mode 100644
index 0000000000000000000000000000000000000000..2fbfca047e1d90fbda3b53a7b5860da30c1bb71e
--- /dev/null
+++ b/msg/relocalization.msg
@@ -0,0 +1,13 @@
+Header header # Msg header.
+
+bool front_icp_check # If front ICP is enough good to relocalization.
+geometry_msgs/Pose2D front_icp_odom # Front laser icp odometry in base_link reference.
+float32[9] front_odom_covariance # Front ICP odometry covariance.
+geometry_msgs/Pose2D front_blink_pose # Robot pose from front ICP.
+float32[9] front_pose_covariance # Robot pose covariance.
+
+bool rear_icp_check # If rear ICP is enough good to relocalization.
+geometry_msgs/Pose2D rear_icp_odom # Rear laser icp odometry in base_link reference.
+float32[9] rear_odom_covariance # Rear ICP odometry covariance.
+geometry_msgs/Pose2D rear_blink_pose # Robot pose from rear ICP.
+float32[9] rear_pose_covariance # Robot pose covariance.
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index de04355224c1e7bf2780bec0568ade207314106e..4ff10fd0d20789e8d1199fae9c01df52c41f2575 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -175,11 +175,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
// rotate angle with tf between blink and laser
- if (this->z_axis_blink_front_laser_inv_)
- this->front_icp_srv_.request.rot_angle_estimation *=-1.0;
- if (this->z_axis_blink_rear_laser_inv_)
- this->rear_icp_srv_.request.rot_angle_estimation *=-1.0;
-
+ ////////////////////////////////////////////////////////7 uncoment////////////////////////////////////////7
+ // if (this->z_axis_blink_front_laser_inv_)
+ // this->front_icp_srv_.request.rot_angle_estimation *=-1.0;
+ // if (this->z_axis_blink_rear_laser_inv_)
+ // this->rear_icp_srv_.request.rot_angle_estimation *=-1.0;
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////7
// ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
this->odom_input_is_ok_ = (this->odom_input_is_ok_ && get_last_odom());
@@ -226,10 +227,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
tf2::Quaternion q;
q.setRPY(0.0, 0.0, front_icp_srv_.response.new_laser_pose.theta);
this->transform_msg.transform.rotation = tf2::toMsg(q);
- // this->transform_msg.transform.rotation.x = q.getRotation().x();
- // this->transform_msg.transform.rotation.y = q.getRotation().y();
- // this->transform_msg.transform.rotation.z = q.getRotation().z();
- // this->transform_msg.transform.rotation.w = q.getRotation().w();
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
@@ -307,9 +304,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
//update collison msg data.
this->collision_msg_.rear_icp_err = rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid;
this->collision_msg_.rear_icp_points = rear_icp_srv_.response.nvalid;
- // this->collision_msg_.rear_laser_disp = rear_icp_srv_.response.new_laser_pose;
- // for (unsigned int i=0; i<9; i++)
- // this->collision_msg_.rear_covariance[i] = rear_icp_srv_.response.covariance[i];
//calculate new base_link pose in fixed frame coordenates
new_laser_pose << rear_icp_srv_.response.new_laser_pose.x, rear_icp_srv_.response.new_laser_pose.y, rear_icp_srv_.response.new_laser_pose.theta;
@@ -331,10 +325,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
tf2::Quaternion q;
q.setRPY(0.0, 0.0, rear_icp_srv_.response.new_laser_pose.theta);
this->transform_msg.transform.rotation = tf2::toMsg(q);
- // this->transform_msg.transform.rotation.x = q.getRotation().x();
- // this->transform_msg.transform.rotation.y = q.getRotation().y();
- // this->transform_msg.transform.rotation.z = q.getRotation().z();
- // this->transform_msg.transform.rotation.w = q.getRotation().w();
this->tf2_broadcaster.sendTransform(this->transform_msg);
}
@@ -467,141 +457,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
}
// [fill msg structures]
- // Initialize the topic message structure
- //this->collisions_collision_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_rear_after_icp_scan_LaserScan_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_front_after_icp_scan_LaserScan_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_rear_icp_pose_PoseWithCovariance_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_front_icp_pose_PoseWithCovariance_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_rear_icp_last_scan_LaserScan_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_rear_icp_ref_scan_LaserScan_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_front_icp_last_scan_LaserScan_msg_.data = my_var;
-
- // Initialize the topic message structure
- //this->debug_front_icp_ref_scan_LaserScan_msg_.data = my_var;
-
-
- /*
- //tf2_listener example BEGIN
- try{
- std::string target_frame = "child_frame";
- std::string source_frame = "parent_frame";
- ros::Time time = ros::Time::now();
- ros::Duration timeout = ros::Duration(0.1);
- this->alg_.unlock();
- bool tf2_exists = this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout);
- this->alg_.lock();
- if(tf2_exists){
- geometry_msgs::PoseStamped stamped_pose_in;
- stamped_pose_in.header.stamp = time;
- stamped_pose_in.header.frame_id = source_frame;
- stamped_pose_in.pose.position.x = 1.0;
- stamped_pose_in.pose.position.y = 0.0;
- stamped_pose_in.pose.position.z = 0.0;
- tf2::Quaternion quat_tf;
- quat_tf.setRPY(0.0, 0.0, 1.5709);
- geometry_msgs::Quaternion quat_msg;
- tf2::convert(quat_tf, quat_msg);
- stamped_pose_in.pose.orientation = quat_msg;
- double yaw, pitch, roll;
- tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
- ROS_INFO("Original pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_in.header.frame_id.c_str(), stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z, yaw, pitch, roll);
- geometry_msgs::PoseStamped stamped_pose_out;
- geometry_msgs::TransformStamped transform;
- transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
- quat_msg = transform.transform.rotation;
- tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
- ROS_INFO("Found transform betwen frames (%s-->%s) with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", source_frame.c_str(), target_frame.c_str(), transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, yaw, pitch, roll);
- tf2::doTransform(stamped_pose_in, stamped_pose_out, transform);
- quat_msg = stamped_pose_out.pose.orientation;
- tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
- ROS_INFO("Transformed pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_out.header.frame_id.c_str(), stamped_pose_out.pose.position.x, stamped_pose_out.pose.position.y, stamped_pose_out.pose.position.z, yaw, pitch, roll);
- ROS_INFO("---");
- }else{
- ROS_WARN("No transform found from '%s' to '%s'", source_frame.c_str(), target_frame.c_str()); }
- }catch (tf2::TransformException &ex){
- ROS_ERROR("TF2 Exception: %s",ex.what()); }
- //tf2_listener example END
- */
-
// [fill srv structure and make request to the server]
- //icp_srv_.request.data = my_var;
- //ROS_INFO("CollisionManagerAlgNode:: Calling service icp_client_!");
- //if (icp_client_.call(icp_srv_))
- //{
- // ROS_INFO("CollisionManagerAlgNode:: icp_client_ received a response from service server");
- //// ROS_INFO("CollisionManagerAlgNode:: Response: %s", icp_srv_.response.somestringfield.c_str());
- //}
- //else
- //{
- // ROS_INFO("CollisionManagerAlgNode:: Failed to call service on topic %s",this->icp_client_.getService().c_str());
- //}
-
-
// [fill action structure and make request to the action server]
// [publish messages]
- // Uncomment the following line to publish the topic message
- //this->collisions_publisher_.publish(this->collisions_collision_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_LaserScan_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_front_after_icp_scan_publisher_.publish(this->debug_front_after_icp_scan_LaserScan_msg_);
-
-
- /*
- //tf2_broadcaster example BEGIN
- this->transform_msg.header.stamp = ros::Time::now();
- this->transform_msg.header.frame_id = "parent_frame";
- this->transform_msg.child_frame_id = "child_frame";
- geometry_msgs::Transform t;
- t.translation.x = 0.0;
- t.translation.y = 0.0;
- t.translation.z = 0.0;
- tf2::Quaternion q;
- q.setRPY(0.0, 0.0, 0.0);
- t.rotation = tf2::toMsg(q);
- this->transform_msg.transform = t;
- this->tf2_broadcaster.sendTransform(this->transform_msg);
- //tf2_broadcaster example END
- */
-
- // Uncomment the following line to publish the topic message
- //this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_PoseWithCovariance_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_PoseWithCovariance_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_rear_icp_last_scan_publisher_.publish(this->debug_rear_icp_last_scan_LaserScan_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_rear_icp_ref_scan_publisher_.publish(this->debug_rear_icp_ref_scan_LaserScan_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_front_icp_last_scan_publisher_.publish(this->debug_front_icp_last_scan_LaserScan_msg_);
-
- // Uncomment the following line to publish the topic message
- //this->debug_front_icp_ref_scan_publisher_.publish(this->debug_front_icp_ref_scan_LaserScan_msg_);
-
this->odom_mutex_exit();
this->front_laser_scan_mutex_exit();