From c34d57154bae80bc5eae2e640054d6b6b20fa216 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 19 Jan 2022 15:24:42 +0100
Subject: [PATCH] Updated launch file from imu_bias_filter odom update
---
launch/collisions.launch | 5 +--
rviz/dogs.rviz | 18 +++------
src/collision_manager_alg_node.cpp | 62 ++++++++++++++++--------------
3 files changed, 42 insertions(+), 43 deletions(-)
diff --git a/launch/collisions.launch b/launch/collisions.launch
index 3b15a7a..928e93b 100644
--- a/launch/collisions.launch
+++ b/launch/collisions.launch
@@ -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>
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index 0ecfb37..62dc341 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -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:
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 6c6c656..5296f3a 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -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);
--
GitLab