Added user node. Error in perception. Fixed default camera

microsoft · Nov 5, 2021 · d0c708b · d0c708b
1 parent a1916b2
commit d0c708b
Show file tree

Hide file tree

Showing 14 changed files with 1,916 additions and 1,684 deletions.
diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
diff --git a/ros/src/airsim_ros_pkgs/msg/IntrinsicsCamera.msg b/ros/src/airsim_ros_pkgs/msg/IntrinsicsCamera.msg
@@ -0,0 +1,3 @@
+float64 focal_length
+float64 aperture
+float64 focus_distance
diff --git a/ros/src/airsim_ros_pkgs/msg/TargetState.msg b/ros/src/airsim_ros_pkgs/msg/TargetState.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header
+string name
+geometry_msgs/Pose pose
diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
@@ -222,20 +222,6 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
 
     vehicle_ros->intrinsics_pub_vec.push_back(camera_publisher);
 
-    IntrinsicsSubscriber camera_subscriber;
-    camera_subscriber.camera_name = "";
-    camera_subscriber.subscriber = nh_private_.subscribe<airsim_ros_pkgs::IntrinsicsCamera>(
-        curr_vehicle_name + "/" + "" + "/set_intrinsics", 1,
-        boost::bind(&AirsimROSWrapper::set_intrinsics_cb, this, _1, curr_vehicle_name, ""));
-    vehicle_ros->intrinsics_sub_vec.push_back(camera_subscriber);
-
-    vehicle_ros->enable_manual_focus_srv_vec.push_back(
-        nh_private_.advertiseService<airsim_ros_pkgs::EnableManualFocus::Request,
-                                     airsim_ros_pkgs::EnableManualFocus::Response>(
-            curr_vehicle_name + "/" + "" + "/" + "enable_manual_focus",
-            boost::bind(&AirsimROSWrapper::enable_manual_focus_srv_cb, this, _1, _2, curr_vehicle_name, "")));
-
-    // iterate over camera map std::map<std::string, CameraSetting> .cameras;
     for (auto& curr_camera_elem : vehicle_setting->cameras)
     {
       auto& camera_setting = curr_camera_elem.second;
@@ -251,13 +237,13 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
       IntrinsicsPublisher camera_publisher;
       camera_publisher.camera_name = curr_camera_name;
       camera_publisher.publisher = nh_private_.advertise<airsim_ros_pkgs::IntrinsicsCamera>(
-          curr_vehicle_name + "/" + curr_camera_name + "/intrinsics", 10);
+          curr_vehicle_name + "/" + curr_camera_name + "/get_intrinsics", 10);
       vehicle_ros->intrinsics_pub_vec.push_back(camera_publisher);
 
       IntrinsicsSubscriber camera_subscriber;
-      camera_subscriber.camera_name = "";
+      camera_subscriber.camera_name = curr_camera_name;
       camera_subscriber.subscriber = nh_private_.subscribe<airsim_ros_pkgs::IntrinsicsCamera>(
-          curr_vehicle_name + "/" + curr_camera_name + "/intrinsics", 1,
+          curr_vehicle_name + "/" + curr_camera_name + "/set_intrinsics", 1,
           boost::bind(&AirsimROSWrapper::set_intrinsics_cb, this, _1, curr_vehicle_name, curr_camera_name));
       vehicle_ros->intrinsics_sub_vec.push_back(camera_subscriber);
 

diff --git a/ros/src/airsim_ros_pkgs/srv/EnableManualFocus.srv b/ros/src/airsim_ros_pkgs/srv/EnableManualFocus.srv
@@ -0,0 +1,3 @@
+bool enable
+---
+bool success
diff --git a/ros/src/airsim_ros_pkgs/srv/MoveOnPath.srv b/ros/src/airsim_ros_pkgs/srv/MoveOnPath.srv
@@ -0,0 +1,9 @@
+#Request : expects position setpoint via x, y, z
+#Request : expects yaw setpoint via yaw (send yaw_valid=true)
+geometry_msgs/Point[] positions
+float64 vel
+float64 timeout
+float64 rads_yaw
+---
+#Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true)
+bool success
diff --git a/ros/src/cinempc/include/cinempc_main_node.h b/ros/src/cinempc/include/cinempc_main_node.h
@@ -4,8 +4,10 @@
 #include <airsim_ros_pkgs/Takeoff.h>
 #include <cinempc/Constraints.h>
 #include <cinempc/GetNextPersonPoses.h>
+#include <cinempc/GetUserConstraints.h>
 #include <cinempc/MPCIncomingState.h>
 #include <cinempc/MPCResult.h>
+#include <cinempc/PerceptionMsg.h>
 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Float32.h>
 #include <stdio.h>
@@ -41,7 +43,8 @@
 #include <eigen3/Eigen/QR>
 #include <random>
 
-ros::Publisher intrinsics_publisher;
+ros::Publisher fpv_intrinsics_publisher;
+ros::Publisher perception_publisher;
 ros::Publisher gimbal_rotation_publisher;
 ros::Publisher new_state_publisher;
 

diff --git a/ros/src/cinempc/include/cinempc_perception_node.h b/ros/src/cinempc/include/cinempc_perception_node.h
@@ -1,4 +1,5 @@
 #include <cinempc/PerceptionMsg.h>
+#include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/Image.h>
 
 #include <DarkHelp.hpp>
@@ -11,4 +12,5 @@
 #include "opencv4/opencv2/opencv.hpp"
 #include "ros/ros.h"
 
-DarkHelp darkhelp;
+DarkHelp darkhelp;
+ros::Publisher perception_result_publisher;
diff --git a/ros/src/cinempc/launch/cine_mpc_all.launch b/ros/src/cinempc/launch/cine_mpc_all.launch
@@ -8,5 +8,8 @@
 <!-- Cinempc solver node -->
 	<include file="$(find cinempc)/launch/cine_mpc_solver_node.launch"/>	
 <!-- Cinempc main node -->
-	<!--<include file="$(find cinempc)/launch/cine_mpc_solver_node.launch"/>	-->
+<!--	<include file="$(find cinempc)/launch/cine_mpc_main_node.launch"/>	-->
+	<node name="cinempc_perception_node" pkg="cinempc" type="cinempc_perception_node"  output="screen"></node>
+	<node name="cinempc_user_node" pkg="cinempc" type="cinempc_user_node"  output="screen">
+	</node>
 </launch>