LaserscanToPointcloud() : filter_chain_("sensor_msgs::LaserScan") { ros::NodeHandle nh_; scan_sub_ = nh_.subscribe("scan", 10, &LaserscanToPointcloud::scanCallback, this); point_cloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("scan_cloud",10,false); ros::NodeHandle pnh_("~"); pnh_.param("max_range", p_max_range_, 29.0); pnh_.param("min_range", p_min_range_, 0.0); filter_chain_.configure("scan_filter_chain", pnh_); pnh_.param("use_high_fidelity_projection", p_use_high_fidelity_projection_, false); if (p_use_high_fidelity_projection_) { pnh_.param("target_frame", p_target_frame_, std::string("NO_TARGET_FRAME_SPECIFIED")); if (p_target_frame_ == "NO_TARGET_FRAME_SPECIFIED") { ROS_ERROR("No target frame specified! Needs to be set for high fidelity projection to work"); p_use_high_fidelity_projection_ = false; return; } tfl_.reset(new tf::TransformListener()); wait_duration_ = ros::Duration(0.5); } }
RotatingCloudToAggregatedCloud() { ros::NodeHandle nh_; prior_roll_angle_ = 0.0; scan_sub_ = nh_.subscribe("cloud", 10, &RotatingCloudToAggregatedCloud::cloudCallback, this); point_cloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_out",10,false); ros::NodeHandle pnh_("~"); pnh_.param("target_frame", p_target_frame_, std::string("base_link")); tfl_.reset(new tf::TransformListener()); wait_duration_ = ros::Duration(0.5); }
int main(int argc, char** argv) { ros::init(argc, argv, "jsk_model_marker_interface"); ros::NodeHandle n; ros::NodeHandle pnh_("~"); tf::TransformListener tfl_; ros::Publisher pub_ = pnh_.advertise<tf::tfMessage> ("/specific_transform", 1); std::vector<std::string> parent_frame; std::vector<std::string> child_frame; { XmlRpc::XmlRpcValue param_val; pnh_.getParam("parent_frame", param_val); if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) { for(int i = 0; i < param_val.size(); i++) { std::string st = param_val[i]; parent_frame.push_back(st); } } else if (param_val.getType() == XmlRpc::XmlRpcValue::TypeString) { std::string st = param_val; parent_frame.push_back(st); } } { XmlRpc::XmlRpcValue param_val; pnh_.getParam("child_frame", param_val); if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) { for(int i = 0; i < param_val.size(); i++) { std::string st = param_val[i]; child_frame.push_back(st); } } else if (param_val.getType() == XmlRpc::XmlRpcValue::TypeString) { std::string st = param_val; child_frame.push_back(st); } } if (parent_frame.size() != child_frame.size()) { ROS_FATAL("size difference between parent frames and child frames"); } double loop_hz; pnh_.param("loop_hz", loop_hz, 1.0 ); for(int i = 0; i < parent_frame.size(); i++) { ROS_INFO_STREAM("parent->child: " << parent_frame[i] << " -> " << child_frame[i]); } ROS_INFO_STREAM("loop_hz:" << loop_hz); ros::Rate rate(loop_hz); while (ros::ok()) { tf::tfMessage tf_msg; for(int i = 0; i < parent_frame.size(); i++) { geometry_msgs::TransformStamped tfs_msg; tf::StampedTransform stf; try { tfl_.lookupTransform(parent_frame[i], child_frame[i], ros::Time(0), stf); tf::transformStampedTFToMsg(stf, tfs_msg); tf_msg.transforms.push_back(tfs_msg); } catch(tf::TransformException ex) { ROS_INFO_STREAM("missing transform: " << parent_frame[i] << " to " << child_frame[i]); } } pub_.publish(tf_msg); ros::spinOnce(); rate.sleep(); } }