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();
}
}
