2 #include <xmlrpcpp/XmlRpc.h>
32 int main(
int argc,
char **argv){
33 ros::init(argc, argv,
"robot_base_calculation_apprioach");
34 std::shared_ptr<ros::NodeHandle> n(
new ros::NodeHandle);
36 std::shared_ptr<Abstract_base> simple_base = std::make_shared<Simple_base>(n);
37 std::shared_ptr<Abstract_base_implementation> simple_base_implementation = std::make_shared<Simple_base_implementation>();
38 std::shared_ptr<Base_calculation_mediator> mediator = std::make_shared<Base_calculation_mediator>(n);
40 simple_base->set_implementation(simple_base_implementation);
42 std::map<const std::string, std::vector<pcl::PointXYZ>> results = simple_base->base_calculation();
44 auto rd = mediator->robot_reader()->robot_data();
45 for (
int i = 0; i < rd.size() ;i++){
46 std::unique_ptr<Abstract_robot> ceti = std::make_unique<Ceti_robot>(rd[i].name_, rd[i].pose_, rd[i].size_);
47 std::unique_ptr<Panda_decorator> ceti_panda = std::make_unique<Panda_decorator>(std::move(ceti));
49 mediator->connect_robots(std::move(ceti_panda));
51 mediator->set_result_vector(results);
57 auto wd = mediator->wing_reader()->wing_data();
59 for (
int i = 0; i < mediator->robots().size(); i++){
62 for(
object_data& w : wd.at(mediator->robots()[i]->name()).first){
64 w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
66 }
catch(
const std::out_of_range& oor) {
67 ROS_INFO(
"No panel data for %s", mediator->robots()[i]->name().c_str());
72 auto ceti_bot =
dynamic_cast<Ceti_robot*
>(mediator->robots()[i]->next());
74 }
catch(
const std::out_of_range& oor) {
75 ROS_INFO(
"No mask data for %s", mediator->robots()[i]->name().c_str());
79 mediator->set_panel();