44 #include <sensor_msgs/PointCloud2.h> 87 const BankArgument & ba,
89 const double mo_old_width)
94 - width_factor * fabs(mo.seen_width - mo_old_width));
103 PointCloud2ArrayInterpreterNodelet::PointCloud2ArrayInterpreterNodelet()
105 PointCloud2InterpreterNodelet::PointCloud2InterpreterNodelet()
110 PointCloud2ArrayInterpreterNode::PointCloud2ArrayInterpreterNode()
112 PointCloud2InterpreterNode::PointCloud2InterpreterNode()
137 PointCloud2ArrayInterpreterNodelet::~PointCloud2ArrayInterpreterNodelet()
140 PointCloud2ArrayInterpreterNode::~PointCloud2ArrayInterpreterNode()
144 PointCloud2InterpreterNodelet::~PointCloud2InterpreterNodelet()
147 PointCloud2InterpreterNode::~PointCloud2InterpreterNode()
151 int nr_banks =
banks.size();
152 for (
int i=0; i<nr_banks; ++i)
167 void PointCloud2ArrayInterpreterNodelet::pointCloud2ArrayCallback(
const find_moving_objects::PointCloud2Array::ConstPtr & msg)
170 void PointCloud2ArrayInterpreterNode::pointCloud2ArrayCallback(
const find_moving_objects::PointCloud2Array::ConstPtr & msg)
191 for (
int i=0; i<msg->msgs.size(); ++i)
201 banks[i]->findAndReportMovingObjects();
213 banks[0]->findAndReportMovingObjects();
229 ROS_DEBUG_STREAM(
"PointCloud2Array sensor is using frame: " << msg->header.frame_id);
234 const int nr_msgs = msg->msgs.size();
235 if (
banks.size() == 0)
241 banks.resize(nr_msgs);
242 for (
int i=0; i<nr_msgs; ++i)
252 for (
int i=0; i<nr_msgs; ++i)
254 const std::string append_str =
"_" + std::to_string(i);
256 bank_arguments[i].topic_objects_closest_point_markers.append(append_str);
257 bank_arguments[i].topic_objects_velocity_arrows.append(append_str);
258 bank_arguments[i].topic_objects_delta_position_lines.append(append_str);
270 for (
int i=0; i<nr_msgs; ++i)
276 if (
banks.size() == 0)
314 bank_arguments[0].nr_scans_in_bank = nr_scans - ((long) nr_scans) == 0.0 ? nr_scans + 1 : ceil(nr_scans);
329 " (based on " <<
received_messages <<
" msgs during " << elapsed_time <<
" seconds)");
334 " (based on " <<
received_messages <<
" msgs during " << elapsed_time <<
" seconds)");
364 ROS_ERROR(
"Message callback is in an unknown state");
375 void PointCloud2ArrayInterpreterNodelet::onInit()
377 void PointCloud2InterpreterNodelet::onInit()
381 nh = getNodeHandle();
382 nh_priv = getPrivateNodeHandle();
386 void PointCloud2ArrayInterpreterNode::onInit()
388 void PointCloud2InterpreterNode::onInit()
404 bank_argument.angle_max /= 2.0;
405 bank_argument.angle_min = -bank_argument.angle_max;
448 if (bank_argument.PC2_threshold_z_max < bank_argument.PC2_threshold_z_min)
450 std::string err =
"threshold_z_max cannot be smaller than threshold_z_min";
482 if (strcmp(bank_argument.map_frame.c_str(), bank_argument.fixed_frame.c_str()) != 0)
486 if (strcmp(bank_argument.map_frame.c_str(), bank_argument.base_frame.c_str()) != 0 &&
487 strcmp(bank_argument.fixed_frame.c_str(), bank_argument.base_frame.c_str()) != 0)
507 tf_filter->registerCallback( boost::bind(
510 &PointCloud2ArrayInterpreterNodelet::pointCloud2ArrayCallback,
517 &PointCloud2ArrayInterpreterNode::pointCloud2ArrayCallback,
533 int main (
int argc,
char ** argv)
540 PointCloud2ArrayInterpreterNode pc2_interpreter;
546 PointCloud2InterpreterNode pc2_interpreter;
double optimize_nr_scans_in_bank
#define NODELET_INFO_STREAM(...)
const bool default_publish_objects_width_lines
const bool default_publish_objects_closest_points_markers
const std::string default_ns_velocity_arrows
#define NODELET_ERROR(...)
const double default_object_threshold_edge_max_delta_range
const bool default_velocity_arrows_use_fixed_frame
const std::string default_message_z_coordinate_field_name
const bool default_publish_objects_delta_position_lines
const std::string default_message_y_coordinate_field_name
BankArgument bank_argument
virtual long init(BankArgument bank_argument, const sensor_msgs::LaserScan *msg)
double max_confidence_for_dt_match
virtual long addMessage(const sensor_msgs::LaserScan *msg)
const double default_threshold_z_max
message_filters::Subscriber< sensor_msgs::LaserScan > * tf_subscriber
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const double default_base_confidence
const double default_bank_view_angle
const std::string default_topic_objects_closest_points_markers
int subscribe_buffer_size
const double default_ema_alpha
const std::string default_subscribe_topic
const bool default_publish_ema
const std::string default_topic_objects
const int default_subscribe_buffer_size
const bool default_velocity_arrows_use_full_gray_scale
const int default_nr_scans_in_bank
const bool default_velocity_arrows_use_base_frame
ROSCPP_DECL void spin(Spinner &spinner)
const std::string default_topic_objects_width_lines
const double default_object_threshold_min_speed
const std::string default_message_x_coordinate_field_name
const double default_max_confidence_for_dt_match
const std::string default_ns_delta_position_lines
const std::string default_ns_width_lines
const double default_threshold_z_min
const int default_object_threshold_max_delta_width_in_points
const double default_delta_width_confidence_decrease_factor
const std::string default_topic_ema
std::vector< BankArgument > bank_arguments
const double default_optimize_nr_scans_in_bank
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const double default_object_threshold_bank_tracking_max_delta_distance
const double default_object_threshold_min_confidence
const double default_voxel_leaf_size
const std::string default_fixed_frame
const int default_object_threshold_min_nr_points
int main(int argc, char **argv)
const std::string default_topic_objects_delta_position_lines
virtual double calculateConfidence(const find_moving_objects::MovingObject &mo, const find_moving_objects::BankArgument &ba, const double delta_time, const double mo_old_width)
const double default_object_threshold_max_distance
const int default_nr_points_per_scan_in_bank
#define ROS_DEBUG_STREAM(args)
const bool default_publish_objects
#define NODELET_DEBUG_STREAM(...)
tf2_ros::Buffer * tf_buffer
tf2_ros::TransformListener * tf_listener
tf2_ros::MessageFilter< sensor_msgs::LaserScan > * tf_filter
const std::string default_base_frame
std::vector< Bank * > banks
const std::string default_map_frame
std::string subscribe_topic
const bool default_velocity_arrows_use_sensor_frame
const bool default_sensor_frame_has_z_axis_forward
const int default_publish_buffer_size
#define ROS_INFO_STREAM(args)
void pointCloud2Callback(const sensor_msgs::PointCloud2::ConstPtr &msg)
const bool default_publish_objects_velocity_arrows
static bool waitForValid()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::vector< std::string > tf_filter_target_frames
const std::string default_topic_objects_velocity_arrows