i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

MapCloudDisplay: fixed default channel for scans. imu_to_tf: fixed wrong stabilized orientation with non null roll/pitch

Browse Source
This commit is contained in:
matlabbe
2019-05-02 19:42:15 -04:00
parent b670186610
commit 2eb8ba1be1
2 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/imu_to_tf.cpp
+2 -3
View File
@@ -85,9 +85,8 @@ private:
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp);
tmp *= st;
st.setRotation(tmp.getRotation());
tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
st *= tmp;
st.child_frame_id_ = baseFrameId_;
}
catch(tf::TransformException & ex)
src/rviz/MapCloudDisplay.cpp
+9 -1
View File
@@ -280,7 +280,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
// Always refresh the cloud if there are data
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
ROS_WARN("s.sensorData().laserScanCompressed()=%d", s.sensorData().laserScanCompressed().size());
if((fromDepth &&
!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
@@ -410,6 +409,7 @@ void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr
bool cur_xyz_valid = false;
bool cur_color_valid = false;
bool has_rgb_transformer = false;
bool has_intensity_transformer = false;
M_TransformerInfo::iterator trans_it = transformers_.begin();
M_TransformerInfo::iterator trans_end = transformers_.end();
for(;trans_it != trans_end; ++trans_it)
@@ -438,6 +438,10 @@ void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr
{
has_rgb_transformer = true;
}
else if (name == "Intensity")
{
has_intensity_transformer = true;
}
color_transformer_property_->addOptionStd( name );
}
}
@@ -458,6 +462,10 @@ void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr
{
color_transformer_property_->setStringStd( "RGB8" );
}
else if (has_intensity_transformer)
{
color_transformer_property_->setStringStd( "Intensity" );
}
else
{
color_transformer_property_->setStringStd( valid_color.rbegin()->second );