WaveguideRectangular::WaveguideRectangular(Magnitude::Magnitude* magn,
const Geometry::Element::Group<const Geometry::Surf>& elem,
const ExcitationMode excMode,
const std::pair<size_t,size_t> mode,
const Bound3& bounds)
: SEMBA::Source::Base(magn),
Geometry::Element::Group<const Geometry::Surf>(elem),
Waveguide(magn, elem, excMode, mode) {
box_ = this->getBound();
if (mode.first == 0 && mode.second == 0) {
printInfo();
throw std::logic_error("At least one mode must be non-zero.");
}
// Computes origin.
origin_ = box_.getMin();
const PhysicalModel::Bound::Bound* bound;
bound = bounds[Math::Constants::x][Math::Constants::L];
if (bound->is<PhysicalModel::Bound::PMC>()) {
origin_(Math::Constants::x) = - box_.getMax()(Math::Constants::x);
}
if (!bound->is<PhysicalModel::Bound::PML>() &&
!bound->is<PhysicalModel::Bound::PMC>()) {
throw std::logic_error(
"Bound termination must be PML or PMC in the x lower axis"
" of the WaveguideRectangular port.");
}
bound = bounds[Math::Constants::y][Math::Constants::L];
if (bound->is<PhysicalModel::Bound::PEC>()) {
origin_(Math::Constants::y) = - box_.getMax()(Math::Constants::y);
}
if (!bound->is<PhysicalModel::Bound::PML>() &&
!bound->is<PhysicalModel::Bound::PEC>()) {
throw std::logic_error(
"Bound termination must be PML or PEC in the y lower axis"
" of the WaveguideRectangular port.");
}
}
void World_wrapper::initialise_origin_orientation(ww::World_wrapper& world_wrapper, const std::string& root_link_name, bool re_initialise){
/* if(re_initialise){
for(auto it = bInitialised.begin(); it != bInitialised.end();it++){
it->second = false;
}
}*/
ros::Rate rate(1.0);
tf::TransformListener listener;
while (!world_wrapper.isInitialised()){
ROS_INFO("initialise world_wrapper");
std::cout<< "num obj2 : " << wrapped_objects.get_number_objects() << std::endl;
std::cout<< "num obj: " << world_wrapper.get_number_objects() << std::endl;
tf::StampedTransform transform;
tf::Vector3 origin, orientation;
geo::fCVec3 origin_,orientation_;
std::string link_name;
std::string traget_frame, source_frame;
try{
for(std::size_t i = 0; i < 5;i++){
link_name = world_wrapper.get_link_name(i);
std::cout<< "object("<<i<<"): " << link_name << std::endl;
traget_frame = "/" + root_link_name;
source_frame = "/" + link_name;
listener.lookupTransform(traget_frame,source_frame, ros::Time(0), transform);
ROS_INFO("target_frame : %s source_frame : %s ",traget_frame.c_str(),source_frame.c_str());
origin = transform.getOrigin();
orientation = transform.getRotation().getAxis();
origin_(0) = origin[0];
origin_(1) = origin[1];
origin_(2) = origin[2];// - 0.02;
orientation_[0] = transform.getRotation().getX();
orientation_[1] = transform.getRotation().getY();
orientation_[2] = transform.getRotation().getZ();
world_wrapper.set_origin_orientation(link_name,origin_,orientation_);
positions[link_name] = std::array<float,6>{{origin_[0],origin_[1],origin_[2],orientation_[0],orientation_[1],orientation_[2]}};
ROS_INFO("transformed %s",link_name.c_str());
}
}catch (tf::TransformException ex){
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
}
ros::spinOnce();
rate.sleep();
}
ROS_INFO("all objects origin and orientation have been set");
}
