publish (and receive in javamg) workcells

launch/mg_routine.launch

+<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>
+
+    

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(){