Flying lead updates

examples/dave_demo_launch/launch/dave_electrical_mate.launch

@@ -15,7 +15,7 @@
         <arg name="gui" value="$(arg gui)"/>
         <arg name="headless" value="false"/>
         <arg name="debug" value="false"/>
-        <arg name="verbose" value="false"/>
+        <arg name="verbose" value="true"/>
     </include>
 
     <include file="$(find uuv_assistants)/launch/publish_world_ned_frame.launch"/>

examples/dave_demo_launch/launch/dave_plug_and_socket_demo.launch

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="gui" default="true"/>
+    <arg name="paused" default="false"/>
+    <arg name="world_name" default="$(find dave_worlds)/worlds/dave_plug_and_socket_demo.world"/>
+    <arg name="set_timeout" default="false"/>
+    <arg name="timeout" default="0.0"/>
+    <arg name="joy_id" default="0"/>
+
+    <!-- use Gazebo's empty_world.launch with dave_ocean_waves.world -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="headless" value="false"/>
+        <arg name="debug" value="false"/>
+        <arg name="verbose" value="true"/>
+    </include>
+
+    <include file="$(find uuv_assistants)/launch/publish_world_ned_frame.launch"/>
+
+    <group if="$(arg set_timeout)">
+        <include file="$(find uuv_assistants)/launch/set_simulation_timer.launch">
+            <arg name="timeout" value="$(arg timeout)"/>
+        </include>
+    </group>
+
+    <include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
+        <arg name="namespace" value="rexrov"/>
+        <arg name="x" value="7.5"/>
+        <arg name="y" value="1.0"/>
+        <arg name="z" value="-95.5"/>
+        <arg name="yaw" value="3.141592"/>
+    </include>
+
+    <include file="$(find uuv_control_cascaded_pid)/launch/joy_velocity.launch">
+        <arg name="uuv_name" value="rexrov" />
+        <arg name="model_name" value="rexrov" />
+        <arg name="joy_id" value="$(arg joy_id)"/>
+    </include>
+
+    <include file="$(find oberon7_control)/launch/joint_control.launch">
+        <arg name="uuv_name" value="rexrov"/>
+    </include>
+
+  <include file="$(find plug_and_socket_description)/launch/upload_socket_platform.launch"/>
+  <node name="spawn_socket" pkg="gazebo_ros" type="spawn_model"
+        args="-urdf -param socket_platform -model socket_platform -x 0 -y 0 -z -95 -Y -1.57079632679" respawn="false" output="screen" />
+
+</launch>

gazebo/dave_gazebo_model_plugins/CMakeLists.txt

@@ -123,3 +123,6 @@ add_dependencies(transponderPlugin dave_gazebo_model_plugins_generate_messages_c
 
 add_library(transceiverPlugin SHARED src/usbl_transceiver_plugin)
 add_dependencies(transceiverPlugin dave_gazebo_model_plugins_generate_messages_cpp)
+
+add_library(plugAndSocketPlugin SHARED src/plug_and_socket_plugin.cc)
+target_link_libraries(plugAndSocketPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

gazebo/dave_gazebo_world_plugins/include/dave_gazebo_world_plugins/uuv_mating.hh → gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/plug_and_socket_plugin.h

@@ -18,7 +18,9 @@
 #ifndef GAZEBO_UUV_MATING_HH_
 #define GAZEBO_UUV_MATING_HH_
 
-#include <ros/ros.h>
+#ifndef DEBUG
+#define DEBUG 0
+#endif
 
 #include <sstream>
 #include <string>
@@ -32,27 +34,42 @@
 
 namespace gazebo
 {
-  class WorldUuvPlugin : public WorldPlugin
+  class PlugAndSocketMatingPlugin : public ModelPlugin
   {
     /// \brief Pointer to the Gazebo world.
     private: physics::WorldPtr world;
 
-    /// \brief Pointer to the plug model.
-    private: physics::ModelPtr plugModel;
-
-    /// \brief Pointer to the plug link.
-    private: physics::LinkPtr plugLink;
+    /// \brief Name of the socket model
+    private: std::string socketModelName;
 
     /// \brief Pointer to the socket model.
     private: physics::ModelPtr socketModel;
 
-    /// \brief Pointer to the hollow tube like structure that receives the plug.
+    /// \brief Name of the socket tube link
+    private: std::string tubeLinkName;
+
+    /// \brief Pointer to the hollow tube like structure that receives the plug
     private: physics::LinkPtr tubeLink;
 
+    /// \brief Name of the sensor plate link name
+    private: std::string sensorPlateName;
+
     /// \brief Pointer to the sensor plate in the socket model that senses when
     /// the plug is inserter.
     private: physics::LinkPtr sensorPlate;
 
+    /// \brief Model name of the plub model
+    private: std::string plugModelName;
+
+    /// \brief Pointer to the plug model.
+    private: physics::ModelPtr plugModel;
+
+    /// \brief Name of the plug link
+    private: std::string plugLinkName;
+
+    /// \brief Pointer to the plug link.
+    private: physics::LinkPtr plugLink;
+
     /// \brief Pinter to the prismatic joint formed between the plug and the
     /// socket.
     private: physics::JointPtr prismaticJoint;
@@ -79,24 +96,32 @@ namespace gazebo
     /// Used to put some buffer between unfreezing and another possible mating.
     private: common::Time unfreezeTimeBuffer = 0;
 
-    /// \brief Roll alignment tolerence.
-    private: double rollAlignmentTolerence;
+    /// \brief Roll alignment tolerance.
+    private: double rollAlignmentTolerance;
 
-    /// \brief pitch alignment tolerence.
-    private: double pitchAlignmentTolerence;
+    /// \brief pitch alignment tolerance.
+    private: double pitchAlignmentTolerance;
 
-    /// \brief Yaw alignment tolerence.
-    private: double yawAlignmentTolerence;
+    /// \brief Yaw alignment tolerance.
+    private: double yawAlignmentTolerance;
 
-    /// \brief Z alignment tolerence.
-    private: double zAlignmentTolerence;
-
-    /// \brief Yaw alignment tolerence.
+    /// \brief force required to mate the plug & socket (lock joint).
     private: double matingForce;
 
-    /// \brief Z alignment tolerence.
+    /// \brief force required to unmate the plug & socket (unlock joint).
     private: double unmatingForce;
 
+    // Some private counter variables ISO "throttled" logging/messages
+
+    /// \brief alignment of plug & socket INFO message
+    private: int alignLogThrottle = 0;
+
+    /// \brief proximity of plug & socket INFO message
+    private: int proximityLogThrottle = 0;
+
+    /// \brief rotational alignment of plug & socket INFO message
+    private: int linksInContactLogThrottle = 0;
+
     /// \brief Concatenates/trims forcesBuffer and timeStamps vectors to
     /// include only the last trimDuration.
     /// \param[in] trimDuration Duration over which to trim the vectors.
@@ -109,52 +134,35 @@ namespace gazebo
     /// \brief Appends another force reading to the forcesBuffer vector.
     void addForce(double force);
 
+    /// \brief Utility function to normalize error angles to (-pi, pi]
+    /// \return normalized angle
+    private: double normalizeError(double error);
+
     /// \brief Constructor.
-    public: WorldUuvPlugin();
+    public: PlugAndSocketMatingPlugin();
 
     /// Documentation inherited.
-    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
+    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
 
     /// \brief Locks the prismatic joint.
     public: void lockJoint(physics::JointPtr prismaticJoint);
 
     /// \brief Release the plug from the joint.
     public: void unfreezeJoint(physics::JointPtr prismaticJoint);
 
-    /// \brief Check that plug and socket are aligned in the roll orientation.
-    /// \return Return true of aligned.
-    private: bool checkRollAlignment(double alignmentThreshold);
-
-    /// \brief Check that plug and socket are aligned in the pitch orientation.
-    /// \return Return true if aligned.
-    private: bool checkPitchAlignment(double alignmentThreshold);
-
-    /// \brief Check that plug and socket are aligned in the yaw orientation.
-    /// \return Return true if aligned.
-    private:bool checkYawAlignment(double alignmentThreshold);
-
-    /// \brief Check that plug and socket are aligned in the all orientations.
-    /// \return Return true if aligned.
-    private: bool checkRotationalAlignment(bool verbose = false);
-
-    /// \brief Check if plug and socket have the same altitude.
-    /// \return Return true if on same altitude.
-    private: bool checkVerticalAlignment(double alignmentThreshold,
-                                         bool verbose = false);
-
     /// \brief Check if plug and socket have the same orientation and altitude.
     /// \return Return true if same r,p,y and z.
-    private: bool isAlligned(bool verbose = false);
+    private: bool isAligned();
 
     /// \brief Measure Euclidean distance between plug an socket.
     /// \return Return true if plug is close to the socket.
-    private: bool checkProximity(bool verbose = false);
+    private: bool checkProximity();
 
     /// \brief Creates the prismatic joint between the socket and plug.
-    private: void construct_joint();
+    private: void constructJoint();
 
     /// \brief Distroys the prismatic joint between the socket and the plug.
-    private: void remove_joint();
+    private: void removeJoint();
 
     /// \brief Calculates the average force exerted by contact2 on contact 1.
     /// \return Average force exerted by contact2 on contact1.
@@ -179,6 +187,6 @@ namespace gazebo
     /// \brief Callback executed at every physics update.
     public: void Update();
   };
-  GZ_REGISTER_WORLD_PLUGIN(WorldUuvPlugin)
+  GZ_REGISTER_MODEL_PLUGIN(PlugAndSocketMatingPlugin)
 }
 #endif