diff --git a/launch/collisions.launch b/launch/collisions.launch index 3b15a7a75abbe624b378c36fa5941500707f2a27..928e93b9edcacac09359e802e16fe76b8f1cc63b 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 0ecfb3722f56c1cc0f8c0f17cfd74850652354d6..62dc34192dccb7a70964b447b27ea6aeb9d7e129 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 6c6c6569ad91a99547212a14e692948dd98853ff..5296f3a591a71e71c352ce1242c0f5fa8757a5c2 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);