Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Skip laserscans with no corresponding tf transform #313

Closed
wants to merge 3 commits into from
Closed

Skip laserscans with no corresponding tf transform #313

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
573a007
Skip laserscans with no corresponding tf transform
Feb 8, 2017
beddaff
Added delay and parameter for tf_frame
Feb 8, 2017
6d7f322
Fixed identation
Feb 10, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,24 +214,24 @@ class scan_unifier_node
void topicCallbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan_in, int scan_id);

/**
* @function checkUnifieCondition
* @brief check in every node-loop if the unifieConditions holds. A unified scan is only published if new laser
* @function checkUnifyCondition
* @brief check in every node-loop if the unifyConditions holds. A unified scan is only published if new laser
* messages from all scanners have been received
*
* input: -
* output: -
*/
void checkUnifieCondition();
void checkUnifyCondition();

/**
* @function unifieLaserScans
* @brief unifie the scan information from all laser scans in vec_laser_struct_
* @function unifyLaserScans
* @brief unify the scan information from all laser scans in vec_laser_struct_
*
* input: -
* output:
* @param: a laser scan message containing unified information from all scanners
*/
sensor_msgs::LaserScan unifieLaserScans();
sensor_msgs::LaserScan unifyLaserScans();

};
#endif
28 changes: 15 additions & 13 deletions cob_scan_unifier/src/scan_unifier_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,14 +220,14 @@ void scan_unifier_node::topicCallbackLaserScan(const sensor_msgs::LaserScan::Con
}

/**
* @function unifieLaserScans
* @brief unifie the scan information from all laser scans in vec_laser_struct_
* @function unifyLaserScans
* @brief unify the scan information from all laser scans in vec_laser_struct_
*
* input: -
* output:
* @param: a laser scan message containing unified information from all scanners
*/
sensor_msgs::LaserScan scan_unifier_node::unifieLaserScans()
sensor_msgs::LaserScan scan_unifier_node::unifyLaserScans()
{
sensor_msgs::LaserScan unified_scan = sensor_msgs::LaserScan();
std::vector<sensor_msgs::PointCloud> vec_cloud;
Expand All @@ -242,11 +242,13 @@ sensor_msgs::LaserScan scan_unifier_node::unifieLaserScans()
ROS_DEBUG_STREAM("Converting scans to point clouds at index: " << i << ", at time: " << vec_laser_struct_.at(i).current_scan_msg.header.stamp << " now: " << ros::Time::now());
try
{
listener_.waitForTransform("/base_link", vec_laser_struct_.at(i).current_scan_msg.header.frame_id,
vec_laser_struct_.at(i).current_scan_msg.header.stamp, ros::Duration(3.0));

if (!listener_.waitForTransform("base_link", vec_laser_struct_.at(i).current_scan_msg.header.frame_id,
vec_laser_struct_.at(i).current_scan_msg.header.stamp, ros::Duration(3.0)))
{
continue;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please correct indentation

ROS_DEBUG("now project to point_cloud");
projector_.transformLaserScanToPointCloud("/base_link",vec_laser_struct_.at(i).current_scan_msg, vec_cloud.at(i), listener_);
projector_.transformLaserScanToPointCloud("base_link",vec_laser_struct_.at(i).current_scan_msg, vec_cloud.at(i), listener_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

could you please make this a parameter, while you are at it?

}
catch(tf::TransformException ex){
ROS_ERROR("%s",ex.what());
Expand All @@ -265,8 +267,8 @@ sensor_msgs::LaserScan scan_unifier_node::unifieLaserScans()
unified_scan.ranges.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1);
unified_scan.intensities.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1);

// now unifie all Scans
ROS_DEBUG("unifie scans");
// now unify all Scans
ROS_DEBUG("unify scans");
for(int j = 0; j < config_.number_input_scans; j++)
{
for (unsigned int i = 0; i < vec_cloud.at(j).points.size(); i++)
Expand Down Expand Up @@ -313,7 +315,7 @@ sensor_msgs::LaserScan scan_unifier_node::unifieLaserScans()
* input: -
* output: -
*/
void scan_unifier_node::checkUnifieCondition()
void scan_unifier_node::checkUnifyCondition()
{
bool all_scans_received = true;
if(!vec_laser_struct_.empty())
Expand All @@ -330,9 +332,9 @@ void scan_unifier_node::checkUnifieCondition()

if(all_scans_received)
{
// all scan-structs received a new msg so now unifie all of them
// all scan-structs received a new msg so now unify all of them
ROS_DEBUG("all_scans_received");
sensor_msgs::LaserScan unified_scan = unifieLaserScans();
sensor_msgs::LaserScan unified_scan = unifyLaserScans();
ROS_DEBUG("now publish");
topicPub_LaserUnified_.publish(unified_scan);
for(int i=0; i < config_.number_input_scans; i++)
Expand All @@ -358,7 +360,7 @@ int main(int argc, char** argv)
while(my_scan_unifier_node.nh_.ok())
{

my_scan_unifier_node.checkUnifieCondition();
my_scan_unifier_node.checkUnifyCondition();

ros::spinOnce();
rate.sleep();
Expand Down