Merge pull request #20 in OSHC/syropod_highlevel_controller from feature/syropod_manipulation to master

* commit 'fc5fe9cb31808b02817aa908e4f8b0db94e55a5a':
  Fixed naming in readme
  Changed syropod_manipulation topic name.
  Standardised naming to primary and secondary within code and readme
  Standardised naming to primary and secondary within code and readme
  Fixed code format
  removed vscode files
  Fixed code formart
  Fixed zero iteration bug
  Updated the formatting to latest style
  Updated the formatting of comments
  Updating formatting to latest style
  Updated readme to include the new features from SyropodManipulation
  Added documentation and comments within code
  added and removed line to see how starting up the robot works
  updating code, added cubic and quardratic bezier equations
  Added new function to move the tip position based on direct cartesian input

Author: Lu, Bethany (Data61, Pullenvale)

.gitignore

@@ -40,6 +40,7 @@
 *.pyc
 *.idea
 *.sublime*
+*.vscode
 
 # Others
 *~

include/syropod_highlevel_controller/admittance_controller.h

@@ -61,13 +61,13 @@ class AdmittanceController
   void updateStiffness(std::shared_ptr<WalkController> walker);
 
 private:
-  std::shared_ptr<Model> model_;  ///< Pointer to the robot model object
-  const Parameters& params_;      ///< Pointer to parameter data structure for storing parameter variables
-  
+  std::shared_ptr<Model> model_; ///< Pointer to the robot model object
+  const Parameters &params_;     ///< Pointer to parameter data structure for storing parameter variables
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-#endif // SYROPOD_HIGHLEVEL_CONTROLLER_ADMITTANCE_CONTROLLER_H
+#endif // SYROPOD_HIGHLEVEL_CONTROLLER_ADMITTANCE_CONTROLLER_H

include/syropod_highlevel_controller/debug_visualiser.h

@@ -25,63 +25,63 @@ class DebugVisualiser
 public:
   /// Constructor for debug output class. Sets up publishers for the visualisation markers and initialises odometry.
   DebugVisualiser(void);
-  
+
   /// Modifier for the time_delta_ member variable.
-  inline void setTimeDelta(const double& time_delta) { time_delta_ = time_delta; };
+  inline void setTimeDelta(const double &time_delta) { time_delta_ = time_delta; };
 
   /// Publishes visualisation markers which represent the robot model for display in RVIZ. Consists of line segments.
   /// linking the origin points of each joint and tip of each leg.
   /// @param[in] model A pointer to the robot model object
   void generateRobotModel(std::shared_ptr<Model> model);
-  
+
   /// Publishes visualisation markers which represent the estimated walking plane.
   /// @param[in] walk_plane A Vector representing the walk plane
   /// @param[in] walk_plane_normal A Vector of the normal to the walk plane
-  void generateWalkPlane(const Eigen::Vector3d& walk_plane, const Eigen::Vector3d& walk_plane_normal);
+  void generateWalkPlane(const Eigen::Vector3d &walk_plane, const Eigen::Vector3d &walk_plane_normal);
 
   /// Publishes visualisation markers which represent the trajectory of the tip of the input leg.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
   void generateTipTrajectory(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent an estimate of the terrain being traversed.
   /// @param[in] model A pointer to the robot model object
   void generateTerrainEstimate(std::shared_ptr<Model> model);
-  
+
   /// Publishes visualisation markers which represent the control nodes of the three bezier curves used to control tip.
   /// trajectory of the input leg.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
   void generateBezierCurves(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent the default tip position of the leg.
   /// @param[in] leg A pointer to a leg of the robot model object
   void generateDefaultTipPositions(std::shared_ptr<Leg> leg);
-  
+
   /// Publises visualisation markers which represent the target tip position of the leg.
   /// @param[in] leg A pointer to a leg of the robot model object
   void generateTargetTipPositions(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent the 2D walkspace for each leg.
   /// @param[in] leg A pointer to a leg of the robot model object
   /// @param[in] walkspace  A map of walkspace radii for a range of bearings to be visualised
-  void generateWalkspace(std::shared_ptr<Leg> leg, const LimitMap& walkspace);
-  
+  void generateWalkspace(std::shared_ptr<Leg> leg, const LimitMap &walkspace);
+
   /// Publishes visualisation markers which represent the 3D workspace for each leg.
   /// @param[in] leg A pointer to a leg of the robot model object
   /// @param[in] body_clearance The vertical offset of the body above the walk plane
-  void generateWorkspace(std::shared_ptr<Leg> leg, const double& body_clearance);
-  
+  void generateWorkspace(std::shared_ptr<Leg> leg, const double &body_clearance);
+
   /// Publishes visualisation markers which represent requested stride vector for each leg.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
   void generateStride(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent the estimated tip force vector for input leg.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
   void generateTipForce(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent the estimated percentage of max torque in each joint.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
   void generateJointTorques(std::shared_ptr<Leg> leg);
-  
+
   /// Publishes visualisation markers which represent the estimate of the gravitational acceleration vector.
   /// @param[in] gravity_estimate An estimate of the gravitational acceleration vector
   void generateGravity(const Eigen::Vector3d& gravity_estimate);
@@ -106,7 +106,7 @@ class DebugVisualiser
   int tip_position_id_ = 0;   ///< Id for tip trajectory markers
   int terrain_marker_id_ = 0; ///< Id for terrain markers
   double marker_scale_ = 0.0; ///< Value used to scale marker sizes based on estimate of robot 'size'
-  
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

include/syropod_highlevel_controller/parameters_and_states.h

@@ -26,12 +26,12 @@ enum SystemState
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 enum RobotState
 {
-  PACKED,             ///< The robot is in a 'packed' state with all joints at defined 'packed' positions
-  READY,              ///< The robot is in a 'ready' state with all joints at defined 'unpacked' positions
-  RUNNING,            ///< The robot is in a 'running' state. This state is where all posing and walking occurs
-  ROBOT_STATE_COUNT,  ///< Misc enum defining number of Robot States
-  UNKNOWN = -1,       ///< The robot is in an initial 'unknown' state, controller will estimate an actual state from it
-  OFF = -2,           ///< The robot is in 'off' state. Only used as alternative to 'running' state for direct start up
+  PACKED,            ///< The robot is in a 'packed' state with all joints at defined 'packed' positions
+  READY,             ///< The robot is in a 'ready' state with all joints at defined 'unpacked' positions
+  RUNNING,           ///< The robot is in a 'running' state. This state is where all posing and walking occurs
+  ROBOT_STATE_COUNT, ///< Misc enum defining number of Robot States
+  UNKNOWN = -1,      ///< The robot is in an initial 'unknown' state, controller will estimate an actual state from it
+  OFF = -2,          ///< The robot is in 'off' state. Only used as alternative to 'running' state for direct start up
 };
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -88,7 +88,7 @@ enum LegState
 {
   WALKING,                ///< The leg is in a 'walking' state - participates in walking cycle
   MANUAL,                 ///< The leg is in a 'manual' state - able to move via manual manipulation inputs
-  LEG_STATE_COUNT,        ///< Misc enum defining number of LegStates  
+  LEG_STATE_COUNT,        ///< Misc enum defining number of LegStates
   WALKING_TO_MANUAL = -1, ///< The leg is in 'walking to manual' state - transitioning from 'walking' to 'manual' state
   MANUAL_TO_WALKING = -2, ///< The leg is in 'manual to walking' state - transitioning from 'manual' to 'walking' state
 };
@@ -199,9 +199,9 @@ struct Parameter
   /// @param[in] name_input The unique name of the parameter to look for on ros parameter server
   /// @param[in] base_parameter_name The base parameter name prepended to 'name_input' common to all parameters
   /// @param[in] required_input Bool denoting if this parameter is required to be initialised
-  inline void init(const std::string& name_input,
-                   const std::string& base_parameter_name = "/syropod/parameters/",
-                   const bool& required_input = true)
+  inline void init(const std::string &name_input,
+                   const std::string &base_parameter_name = "/syropod/parameters/",
+                   const bool &required_input = true)
   {
     ros::NodeHandle n;
     name = name_input;
@@ -211,10 +211,10 @@ struct Parameter
                    " Check config file is loaded and type is correct\n", name.c_str());
   }
 
-  std::string name;              ///< Name of the parameter
-  T data;                        ///< Data which defines parameter
-  bool required = true;          ///< Denotes if this parameter is required to be initialised
-  bool initialised = false;      ///< Denotes if this parameter has been initialised
+  std::string name;         ///< Name of the parameter
+  T data;                   ///< Data which defines parameter
+  bool required = true;     ///< Denotes if this parameter is required to be initialised
+  bool initialised = false; ///< Denotes if this parameter has been initialised
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -383,4 +383,3 @@ struct Parameters
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 #endif // SYROPOD_HIGHLEVEL_CONTROLLER_PARAMETERS_AND_STATES_H
-