Skip to content
Snippets Groups Projects
Commit c34d5715 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Updated launch file from imu_bias_filter odom update

parent cdf7cdc1
No related branches found
No related tags found
No related merge requests found
......@@ -16,7 +16,7 @@
<arg name="bias_filter_config_file" default="$(find iri_imu_bias_filter)/config/params.yaml"/>
<arg name="imu_raw_topic" default="/bno055_imu/imu"/>
<arg name="imu_filtered_topic" default="/bno055_imu/imu_bias_filtered"/>
<arg name="cmd_vel_topic" default="/cmd_vel"/>
<arg name="odom_topic" default="/odom"/>
<arg name="update_bias_ns" default="~/update_bias"/>
<!-- Laser Scan ICP parameters -->
......@@ -27,7 +27,6 @@
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/>
<arg name="front_laser_topic" default="/laser_front/scan"/>
<arg name="rear_laser_topic" default="/laser_rear/scan"/>
<arg name="odom_topic" default="/odom"/>
<include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
if="$(arg imu_driver)">
......@@ -45,7 +44,7 @@
<arg name="config_file" value="$(arg bias_filter_config_file)"/>
<arg name="imu_topic_in" value="$(arg imu_raw_topic)"/>
<arg name="imu_topic_out" value="$(arg imu_filtered_topic)"/>
<arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="update_bias_ns" value="$(arg update_bias_ns)"/>
</include>
......
......@@ -54,10 +54,6 @@ Visualization Manager:
Value: true
map:
Value: true
new_front_laser:
Value: true
new_rear_laser:
Value: true
odom:
Value: true
Marker Scale: 1
......@@ -70,11 +66,9 @@ Visualization Manager:
odom:
base_link:
base_laser_front_link:
new_front_laser:
{}
{}
base_laser_rear_link:
new_rear_laser:
{}
{}
imu_bno055:
{}
Update Interval: 0
......@@ -119,7 +113,7 @@ Visualization Manager:
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
......@@ -134,7 +128,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
......@@ -147,7 +141,7 @@ Visualization Manager:
Color: 252; 233; 79
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
......@@ -162,7 +156,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
......
......@@ -171,20 +171,23 @@ void CollisionManagerAlgNode::mainNodeThread(void)
front_icp_srv_.response.covariance[6], front_icp_srv_.response.covariance[7], front_icp_srv_.response.covariance[8];
this->front_icp_succedded_ = true;
this->transform_msg.header.frame_id = this->front_laser_frame_;
this->transform_msg.child_frame_id = "new_front_laser";
this->transform_msg.header.stamp = this->imu_stamp_;
this->transform_msg.transform.translation.x = front_icp_srv_.response.new_laser_pose.x;
this->transform_msg.transform.translation.y = front_icp_srv_.response.new_laser_pose.y;
this->transform_msg.transform.translation.z = 0.0;
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);
if (this->config_.debug)
{
this->transform_msg.header.frame_id = this->front_laser_frame_;
this->transform_msg.child_frame_id = "new_front_laser";
this->transform_msg.header.stamp = this->imu_stamp_;
this->transform_msg.transform.translation.x = front_icp_srv_.response.new_laser_pose.x;
this->transform_msg.transform.translation.y = front_icp_srv_.response.new_laser_pose.y;
this->transform_msg.transform.translation.z = 0.0;
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);
}
this->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
......@@ -241,20 +244,23 @@ void CollisionManagerAlgNode::mainNodeThread(void)
rear_icp_srv_.response.covariance[6], rear_icp_srv_.response.covariance[7], rear_icp_srv_.response.covariance[8];
this->rear_icp_succedded_ = true;
this->transform_msg.header.frame_id = this->rear_laser_frame_;
this->transform_msg.child_frame_id = "new_rear_laser";
this->transform_msg.header.stamp = this->imu_stamp_;
this->transform_msg.transform.translation.x = rear_icp_srv_.response.new_laser_pose.x;
this->transform_msg.transform.translation.y = rear_icp_srv_.response.new_laser_pose.y;
this->transform_msg.transform.translation.z = 0.0;
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);
if (this->config_.debug)
{
this->transform_msg.header.frame_id = this->rear_laser_frame_;
this->transform_msg.child_frame_id = "new_rear_laser";
this->transform_msg.header.stamp = this->imu_stamp_;
this->transform_msg.transform.translation.x = rear_icp_srv_.response.new_laser_pose.x;
this->transform_msg.transform.translation.y = rear_icp_srv_.response.new_laser_pose.y;
this->transform_msg.transform.translation.z = 0.0;
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);
}
this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment