From fe0fb0a21b873e47d9dd8bc895b1a3b8f7584c5c Mon Sep 17 00:00:00 2001 From: KingMaZito <matteo.aneddama@icloud.com> Date: Sun, 2 Jul 2023 01:04:01 +0200 Subject: [PATCH] publish (and receive in javamg) workcells --- launch/mg_routine.launch | 25 +++++++++++++++++ src/mediator/mg_mediator.cpp | 52 ++++++++++++++++++++++++------------ 2 files changed, 60 insertions(+), 17 deletions(-) create mode 100644 launch/mg_routine.launch diff --git a/launch/mg_routine.launch b/launch/mg_routine.launch new file mode 100644 index 0000000..923292c --- /dev/null +++ b/launch/mg_routine.launch @@ -0,0 +1,25 @@ +<launch> + <arg name="result" default="dummy/-2045918175/-2045918175.yaml" /> + <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> + + <!-- this is to change--> + <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> + <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam> + + <include file="$(find ceti_double)/launch/demo.launch"> + <arg name="use_rviz" value="false"/> + <arg name="scene" value="$(arg result)" /> + </include> + + <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" /> + <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" /> + + <include file="$(find ceti_double)/launch/moveit_rviz.launch"> + <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" /> + </include> + + <node pkg="multi_cell_builder" type="mg_routine" name="mg_routine" output="screen" required="true"> + </node> +</launch> + + diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp index fd87e8c..7f70d13 100644 --- a/src/mediator/mg_mediator.cpp +++ b/src/mediator/mg_mediator.cpp @@ -4,7 +4,7 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) : AbstractMediator(nh) , Controller(*nh.get()) { - addConnection(std::make_shared<MqttConnection>("", "super")); + addConnection(std::make_shared<MqttConnection>("localhost:1883", "asdfadf")); } void MGMediator::mediate(){ @@ -38,31 +38,49 @@ void MGMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot){ void MGMediator::publishTables(){ ros::WallDuration sleep_time(1.0); - - for (const auto& s_r : robots_){ - auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next()); - - for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){ - if(ceti_bot->observerMask()[k]){ - auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next()); - psi_->applyCollisionObject(wmd->marker()); - acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>())); - sleep_time.sleep(); - } - } - } + // for (const auto& s_r : robots_){ + // auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next()); + + // for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){ + // if(ceti_bot->observerMask()[k]){ + // auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next()); + // psi_->applyCollisionObject(wmd->marker()); + // acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>())); + // sleep_time.sleep(); + // } + // } + // } std::string str; std::map<const std::string, std::vector<Cuboid>> robot_to_cuboid; - std::bitset<3> panel_mask; + std::bitset<3> panel_mask(7); for (const auto& pair : robots_) { robot_to_cuboid.insert_or_assign(pair.first, std::vector<Cuboid>()); + std::stringstream ss; + ss << "{\"objects\": [{\"id\": \""<< pair.first << "\", \"type\": \"ARM\"}"; for (auto& c: cuboid_reader_->cuboidBox()){ tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z)); - if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) robot_to_cuboid[pair.first].push_back(c); + if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) { + ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BOX\",\"pos\": {\"x\": "<< c.Pose.position.x << ",\"y\": "<< c.Pose.position.y <<",\"z\": " << c.Pose.position.x << "},\"size\": {\"length\": "<< c.x_depth <<",\"width\": "<< c.y_width <<",\"height\": "<< c.z_heigth <<"},\"orientation\": {\"x\": "<< c.Pose.orientation.x <<", \"y\": "<< c.Pose.orientation.y <<" , \"z\": "<< c.Pose.orientation.z <<", \"w\": "<< c.Pose.orientation.w<< "},"; + ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}"; + } } - } + + for (auto& c: cuboid_reader_->cuboidObstacle()){ + tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z)); + if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) { + ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BIN\",\"pos\": {\"x\": "<< c.Pose.position.x << ",\"y\": "<< c.Pose.position.y <<",\"z\": " << c.Pose.position.x << "},\"size\": {\"length\": "<< c.x_depth <<",\"width\": "<< c.y_width <<",\"height\": "<< c.z_heigth <<"},\"orientation\": {\"x\": "<< c.Pose.orientation.x <<", \"y\": "<< c.Pose.orientation.y <<" , \"z\": "<< c.Pose.orientation.z <<", \"w\": "<< c.Pose.orientation.w<< "},"; + ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}"; + } + } + ss << "]}"; + + // publish + sendToAll("World/robotarm1robotarm2", ss.str()); + } + + } void MGMediator::setPanel(){ -- GitLab