[Spinal][Config] Additional features and new configuration for Spinal

aerial_robot_model/include/aerial_robot_model/servo_bridge.h

@@ -50,6 +50,8 @@
 #include <spinal/ServoStates.h>
 #include <spinal/ServoControlCmd.h>
 #include <spinal/ServoTorqueCmd.h>
+#include <spinal/JointProfiles.h>
+#include <spinal/UavInfo.h>
 #include <controller_manager_msgs/LoadController.h>
 #include <controller_manager_msgs/SwitchController.h>
 #include <urdf/model.h>
@@ -200,6 +202,8 @@ class ServoBridge
 
   ros::Publisher servo_states_pub_;
   ros::Publisher mujoco_control_input_pub_;
+  ros::Publisher joint_profile_pub_;
+  ros::Subscriber uav_info_sub_;
   map<string, ros::Subscriber> servo_states_subs_;
   map<string, ros::Subscriber> servo_ctrl_subs_;
   map<string, bool> no_real_state_flags_;
@@ -219,6 +223,7 @@ class ServoBridge
   void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name);
   void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
   bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
+  void uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg);
 
 public:
   ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp);

aerial_robot_model/src/servo_bridge/servo_bridge.cpp

@@ -73,12 +73,16 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
   mujoco_control_input_pub_ = nh_.advertise<sensor_msgs::JointState>("mujoco/ctrl_input", 1);
   /* common publisher: torque on/off command */
   servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoTorqueCmd>(torque_pub_topic, 1)));
+  /* common publisher: joint profiles */
+  joint_profile_pub_ = nh_.advertise<spinal::JointProfiles>("joint_profiles",1);
+  /* subscriber: uav info */
+  uav_info_sub_ = nh_.subscribe<spinal::UavInfo>("uav_info", 1, &ServoBridge::uavInfoCallback, this);
 
 
   /* get additional config for servos from ros parameters */
   XmlRpc::XmlRpcValue all_servos_params;
   nh_.getParam("servo_controller", all_servos_params);
-
+  spinal::JointProfiles joint_profiles_msg;
   for(auto servo_group_params: all_servos_params)
     {
       if (servo_group_params.second.getType() != XmlRpc::XmlRpcValue::TypeStruct)
@@ -126,7 +130,8 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
               double lower_limit = urdf_model.getJoint(servo_params.second["name"])->limits->lower;
 
               /* get parameters from rosparam */
-              int angle_sgn = servo_params.second.hasMember("angle_sgn")?
+              int servo_id = servo_params.second["id"]; 
+              int angle_sgn = servo_params.second.hasMember("angle_sgn")? 
                 servo_params.second["angle_sgn"]:servo_group_params.second["angle_sgn"];
               int zero_point_offset = servo_params.second.hasMember("zero_point_offset")?
                 servo_params.second["zero_point_offset"]:servo_group_params.second["zero_point_offset"];
@@ -391,3 +396,29 @@ bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_s
   return true;
 }
 
+void ServoBridge::uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg)
+{
+  /* Send servo profiles to Spinal*/
+  spinal::JointProfiles joint_profiles_msg;
+  for(auto servo_group : servos_handler_){
+    for(auto servo : servo_group.second){
+      spinal::JointProfile joint_profile;
+      if(servo_group.first == "joints"){
+        joint_profile.type = spinal::JointProfile::JOINT;
+      }
+      else if(servo_group.first == "gimbals"){
+        joint_profile.type = spinal::JointProfile::GIMBAL;
+      }
+      else{
+        ROS_ERROR("Invalid servo type. Please define 'joints' or 'gimbals'.");
+        continue;
+      }
+      joint_profile.servo_id = servo->getId();
+      joint_profile.angle_sgn = servo->getAngleSgn();
+      joint_profile.angle_scale = servo->getAngleScale();
+      joint_profile.zero_point_offset = servo->getZeroPointOffset();
+      joint_profiles_msg.joints.push_back(joint_profile);
+    }
+  }
+  joint_profile_pub_.publish(joint_profiles_msg);
+}

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -44,6 +44,10 @@ add_message_files(
   FlightConfigCmd.msg
   Vector3Int16.msg
   TorqueAllocationMatrixInv.msg
+  ESCTelemetry.msg
+  ESCTelemetryArray.msg
+  JointProfile.msg
+  JointProfiles.msg
   )
 
 add_service_files(
@@ -53,6 +57,7 @@ add_service_files(
   SetAttitudeGains.srv
   ImuCalib.srv
   MagDeclination.srv
+  SetDirectServoConfig.srv
 )
 
 generate_messages(
@@ -97,6 +102,10 @@ add_dependencies(${PROJECT_NAME}_generate_stm32f7_ros_lib ${catkin_EXPORTED_TARG
 add_custom_target(${PROJECT_NAME}_generate_stm32h7_ros_lib ALL COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh ${PYTHON_EXECUTABLE}  ${PROJECT_SOURCE_DIR}/scripts/make_libraries.py --save_path ${PROJECT_SOURCE_DIR}/mcu_project/boards/stm32H7 --support_rtos --support_ethernet)
 add_dependencies(${PROJECT_NAME}_generate_stm32h7_ros_lib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
 
+# generate ros_lib for STM32H7_v2
+add_custom_target(${PROJECT_NAME}_generate_stm32h7_v2_ros_lib ALL COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh ${PYTHON_EXECUTABLE}  ${PROJECT_SOURCE_DIR}/scripts/make_libraries.py --save_path ${PROJECT_SOURCE_DIR}/mcu_project/boards/stm32H7_v2 --support_rtos --support_ethernet)
+add_dependencies(${PROJECT_NAME}_generate_stm32h7_v2_ros_lib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+
 
 install(DIRECTORY ${SPINAL_DIRS}
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})