Develop (#31)

* Update catkin_ws

* Use interval client

* Add target publisher for obstacle_1

* Add utility script

* Update submodules

catkin_ws/src/simulator_utils/CMakeLists.txt

@@ -136,6 +136,7 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(obstacle_publisher_node src/obstacle_publisher.cpp)
+add_executable(target_publisher_node src/target_publisher.cpp)
 add_executable(mocap_publisher_node src/mocap_publisher.cpp)
 
 ## Rename C++ executable without prefix
@@ -147,10 +148,12 @@ add_executable(mocap_publisher_node src/mocap_publisher.cpp)
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(obstacle_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(target_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(mocap_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(obstacle_publisher_node ${catkin_LIBRARIES})
+target_link_libraries(target_publisher_node ${catkin_LIBRARIES})
 target_link_libraries(mocap_publisher_node ${catkin_LIBRARIES})
 
 #############

catkin_ws/src/simulator_utils/launch/jetson.launch

@@ -5,14 +5,16 @@
 
   <include file="$(find ascend_utils)/launch/jetson.launch">
     <arg name="namespace" value="$(arg namespace)"/>
-    <arg name="use_fluid_fsm" value="true"/>
-    <arg name="fluid_fsm_fcu_url" value="udp://$(env ROS_IP):14540@$(arg px4_addr):14557"/>
+    <arg name="use_fluid_fsm" value="false"/>
   </include>
 
   <group ns="$(arg namespace)">
-    <node name="fluid_fsm_client" pkg="fluid_fsm" type="test_client" output="screen"/>
+    <include file="$(find fluid_fsm)/launch/server_gazebo.launch">
+      <arg name="fcu_url" value="udp://$(env ROS_IP):14540@$(arg px4_addr):14557"/>
+    </include>
 
     <node name="mocap_publisher" pkg="simulator_utils" type="mocap_publisher_node" output="screen"/>
     <node name="obstacle_publisher" pkg="simulator_utils" type="obstacle_publisher_node" output="screen"/>
+    <node name="target_publisher" pkg="simulator_utils" type="target_publisher_node" output="screen"/>
   </group>
 </launch>

catkin_ws/src/simulator_utils/src/mocap_publisher.cpp

@@ -37,7 +37,7 @@ void gazeboModelStatesCallback(gazebo_msgs::ModelStates::ConstPtr msg) {
 int main(int argc, char** argv) {
   ros::init(argc, argv, "mocap_node");
   ros::NodeHandle nh;
-  ros::Subscriber sub = nh.subscribe("gazebo/model_states", 1, gazeboModelStatesCallback);
+  ros::Subscriber sub = nh.subscribe("/gazebo/model_states", 1, gazeboModelStatesCallback);
 
   //std::vector<std::string> droneNames = {"alpha", "bravo"};
   std::vector<DronePosePub> dronePubs;

catkin_ws/src/simulator_utils/src/target_publisher.cpp

@@ -0,0 +1,46 @@
+#include <ros/ros.h>
+#include <gazebo_msgs/ModelStates.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Point32.h>
+#include <std_msgs/String.h>
+#include <ascend_msgs/ObstacleBoundingBoxes.h>
+#include <functional>
+#include <string>
+
+gazebo_msgs::ModelStates::ConstPtr modelStatesMsg;
+void gazeboModelStatesCallback(gazebo_msgs::ModelStates::ConstPtr msg) {
+  modelStatesMsg = msg;
+}
+
+const std::string model_states_topic = "/gazebo/model_states";
+const std::string target_topic = "perception/target";
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "close_obstacle_detector");
+  ros::NodeHandle n;
+  ros::Publisher targetPub = n.advertise<geometry_msgs::Pose>(target_topic, 1);
+  ros::Subscriber gazeboSub = n.subscribe(model_states_topic, 10, gazeboModelStatesCallback);
+
+  ros::topic::waitForMessage<gazebo_msgs::ModelStates>(model_states_topic, n);
+  ros::spinOnce();
+
+  ros::Rate rate(10.f);
+  while (ros::ok()) {
+    ros::spinOnce();
+    rate.sleep();
+
+    geometry_msgs::Pose targetMsg;
+
+    const int len = modelStatesMsg->name.size();
+    const auto& name = modelStatesMsg->name;
+    const auto& pose = modelStatesMsg->pose;
+    for (int i = 0; i < len; i++) {
+      if (name[i] == "obstacle_1") {
+        targetPub.publish(pose[i]);
+      }
+    }  
+  }
+
+  return 0;
+}
+

scripts/start_pc_node.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+containerID=$(docker run -it -d \
+  -e ROS_MASTER_URI=http://offboard_computer:11311 \
+  -e ROS_NAMESPACE=alpha \
+  ascendntnu/control_simulator_pc_node bash)
+docker network connect control_simulator_default $containerID
+echo $containerID
+docker attach $containerID