Fixed code formart

Author: Lu, Bethany(Data61, Pullenvale)

.gitignore

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

include/syropod_highlevel_controller/parameters_and_states.h

@@ -270,7 +270,7 @@ struct AdjustableParameter : public Parameter<std::map<std::string, double>>
 typedef std::map<ParameterSelection, AdjustableParameter*> AdjustableMapType;
 struct Parameters
 {
-   AdjustableMapType adjustable_map;  ///< Map between adjustable parameter designations and associated Parameter object
+  AdjustableMapType adjustable_map;  ///< Map between adjustable parameter designations and associated Parameter object
 
   // Control parameters
   Parameter<double> time_delta;        ///< The period of time between successive ros cycles

include/syropod_highlevel_controller/standard_includes.h

@@ -389,7 +389,7 @@ inline Eigen::Vector3d cubicBezierCurveThroughControlPoint(const T *points, cons
   }
   else
   {
-    ROS_INFO("Something terribly wrong is happening. New Control points for the Cubic Bezier is NOT being generated as expected.");
+    ROS_WARN("Something terribly wrong is happening. New Control points for the Cubic Bezier is NOT being generated as expected.");
     return Eigen::Vector3d(0, 0, 0);
   }
 }
@@ -453,7 +453,7 @@ inline Eigen::Vector3d quarticBezierCurveThroughControlPoint(const T *points, co
   }
   else
   {
-    ROS_INFO("Something terribly wrong. New Control points for the Quartic Bezier is NOT being generated as expected.");
+    ROS_WARN("Something terribly wrong. New Control points for the Quartic Bezier is NOT being generated as expected.");
     return Eigen::Vector3d(0, 0, 0);
   }
 }

include/syropod_highlevel_controller/state_controller.h

@@ -220,12 +220,12 @@ class StateController
   /// Callback for the input manual tip position (in cartesian space) for the AR leg (front right leg)
   /// @param[in] input The Pose geometry message provided by the subscribed topic "/syropod_manipulation/AR/Pose"
   /// @see (https://confluence.csiro.au/display/CPS/Weaver+Diagrams)
-  void arTipPoseInputCallback(const geometry_msgs::Pose &msg);
+  void  primaryTipPoseInputCallback(const geometry_msgs::Pose &msg);
 
   /// Callback for the input manual tip position (in cartesian space) for the AL leg (front left leg).
   /// @param[in] input The Pose geometry message provided by the subscribed topic "/syropod_manipulation/AL/Pose"
   /// @see (https://confluence.csiro.au/display/CPS/Weaver+Diagrams)
-  void alTipPoseInputCallback(const geometry_msgs::Pose &msg);
+  void  secondaryTipPoseInputCallback(const geometry_msgs::Pose &msg);
 
   /// Callback handling the desired parameter selection and sending state messages to user interface.
   /// @param[in] input The Int8 standard message provided by the subscribed topic "syropod_remote/parameter_selection"
@@ -291,8 +291,8 @@ class StateController
   ros::Subscriber parameter_selection_subscriber_;     ///< Subscriber for topic /syropod_remote/parameter_selection
   ros::Subscriber parameter_adjustment_subscriber_;    ///< Subscriber for topic /syropod_remote/parameter_adjustment
 
-  ros::Subscriber ar_tip_pose_subscriber_; ///< Subscriber for topic /syropod_manipulation/AR/Pose
-  ros::Subscriber al_tip_pose_subscriber_; ///< Subscriber for topic /syropod_manipulation/AL/Pose
+  ros::Subscriber primary_tip_pose_subscriber_; ///< Subscriber for topic /syropod_manipulation/AR/Pose
+  ros::Subscriber secondary_tip_pose_subscriber_; ///< Subscriber for topic /syropod_manipulation/AL/Pose
 
   ros::Subscriber target_configuration_subscriber_; ///< Subscriber for topic /target_configuration
   ros::Subscriber target_body_pose_subscriber_;     ///< Subscriber for topic /target_body_pose
@@ -373,8 +373,8 @@ class StateController
   LegContainer::iterator leg_it_;     ///< Leg iteration member variable used to minimise code
   JointContainer::iterator joint_it_; ///< Joint iteration member variable used to minimise code
 
-  Pose ar_pose_input_; ///< Input for the desired pose of the leg tip AR
-  Pose al_pose_input_; ///< Input for the desired pose of the leg tip AR
+  Pose primary_pose_input_; ///< Input for the desired pose of the leg tip AR
+  Pose secondary_pose_input_; ///< Input for the desired pose of the leg tip AR
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

include/syropod_highlevel_controller/walk_controller.h

@@ -220,8 +220,8 @@ class WalkController : public std::enable_shared_from_this<WalkController>
   /// @param[in] secondary_leg_selection_ID The designation of a leg selected (in the secondary role) for manipulation.
   /// @param[in] secondary_tip_pose_input  The cartesian input to move the secondary leg tip to a desired pose.
   /// @todo add orientation tip control and update the comment. WIP
-  void controlManualPose(const int &primary_leg_selection_ID, const Pose &primary_tip_pose_input,
-                         const int &secondary_leg_selection_ID, const Pose &secondary_tip_pose_input);
+  void updateManual(const int &primary_leg_selection_ID, const Pose &primary_tip_pose_input,
+                    const int &secondary_leg_selection_ID, const Pose &secondary_tip_pose_input);
 
   /// Calculates a estimated walk plane which best fits the default tip positions of legs in model.
   /// Walk plane vector in form: [a, b, c] where plane equation equals: ax + by + c = z.
@@ -456,7 +456,7 @@ class LegStepper
   /// Calculates the lateral change in distance from identity tip position to new default tip position for a leg.
   /// @return The change from identity tip position to the new default tip position
   Eigen::Vector3d calculateStanceSpanChange(void);
-  
+
   /// Update Default Tip Position based on external definitions, stance span or tip position at beginning of stance.
   void updateDefaultTipPosition(void);
 
@@ -512,8 +512,8 @@ class LegStepper
   Eigen::Vector3d stride_vector_;     ///< The desired stride vector
   Eigen::Vector3d swing_clearance_;   ///< Position relative to the default tip position to achieve during swing period
 
-  double swing_delta_t_ = 0.0;    
-  double stance_delta_t_ = 0.0;   
+  double swing_delta_t_ = 0.0;
+  double stance_delta_t_ = 0.0;
 
   Pose identity_tip_pose_; ///< The user defined tip pose assuming a identity walk plane
   Pose default_tip_pose_;  ///< The default tip pose per the walk controller, updated with walk plane

src/main.cpp

@@ -17,7 +17,7 @@
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 int main(int argc, char* argv[])
 {
- ros::init(argc, argv, "shc");
+  ros::init(argc, argv, "shc");
   ros::NodeHandle n;
 
   StateController state;
@@ -51,7 +51,7 @@ int main(int argc, char* argv[])
     ros::console::notifyLoggerLevelsChanged();
   }
 
-   // Set ros rate from params
+  // Set ros rate from params
   ros::Rate r(roundToInt(1.0 / params.time_delta.data));
 
   // Wait specified time to aquire all published joint positions via callback

src/model.cpp

@@ -14,7 +14,12 @@
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Model::Model(const Parameters &params, std::shared_ptr<DebugVisualiser> debug_visualiser)
-    : params_(params), debug_visualiser_(debug_visualiser), leg_count_(params_.leg_id.data.size()), time_delta_(params_.time_delta.data), current_pose_(Pose::Identity()), default_pose_(Pose::Identity())
+    : params_(params)
+    , debug_visualiser_(debug_visualiser)
+    , leg_count_(params_.leg_id.data.size())
+    , time_delta_(params_.time_delta.data)
+    , current_pose_(Pose::Identity())
+    , default_pose_(Pose::Identity())
 {
   imu_data_.orientation = UNDEFINED_ROTATION;
   imu_data_.linear_acceleration = Eigen::Vector3d::Zero();
@@ -24,7 +29,13 @@ Model::Model(const Parameters &params, std::shared_ptr<DebugVisualiser> debug_vi
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Model::Model(std::shared_ptr<Model> model)
-    : params_(model->params_), debug_visualiser_(model->debug_visualiser_), leg_count_(model->leg_count_), time_delta_(model->time_delta_), current_pose_(model->current_pose_), default_pose_(model->default_pose_), imu_data_(model->imu_data_)
+    : params_(model->params_)
+    , debug_visualiser_(model->debug_visualiser_)
+    , leg_count_(model->leg_count_)
+    , time_delta_(model->time_delta_)
+    , current_pose_(model->current_pose_)
+    , default_pose_(model->default_pose_)
+    , imu_data_(model->imu_data_)
 {
 }
 
@@ -156,7 +167,13 @@ Eigen::Vector3d Model::estimateGravity(void)
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Leg::Leg(std::shared_ptr<Model> model, const int &id_number, const Parameters &params)
-    : model_(model), params_(params), id_number_(id_number), id_name_(params_.leg_id.data.at(id_number)), joint_count_(params_.leg_DOF.data.at(id_name_)), leg_state_(WALKING), admittance_delta_(Eigen::Vector3d::Zero()), admittance_state_(std::vector<double>(2))
+    : model_(model), params_(params)
+    , id_number_(id_number)
+    , id_name_(params_.leg_id.data.at(id_number))
+    , joint_count_(params_.leg_DOF.data.at(id_name_))
+    , leg_state_(WALKING)
+    , admittance_delta_(Eigen::Vector3d::Zero())
+    , admittance_state_(std::vector<double>(2))
 {
   desired_tip_pose_ = Pose::Undefined();
   desired_tip_velocity_ = Eigen::Vector3d::Zero();
@@ -173,7 +190,12 @@ Leg::Leg(std::shared_ptr<Model> model, const int &id_number, const Parameters &p
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Leg::Leg(std::shared_ptr<Leg> leg, std::shared_ptr<Model> model)
-    : params_(leg->params_), id_number_(leg->id_number_), id_name_(leg->id_name_), joint_count_(leg->joint_count_), leg_state_(leg->leg_state_), admittance_state_(leg->admittance_state_)
+    : params_(leg->params_)
+    , id_number_(leg->id_number_)
+    , id_name_(leg->id_name_)
+    , joint_count_(leg->joint_count_)
+    , leg_state_(leg->leg_state_)
+    , admittance_state_(leg->admittance_state_)
 {
   model_ = (model == NULL ? leg->model_ : model);
   leg_state_publisher_ = leg->leg_state_publisher_;
@@ -348,7 +370,6 @@ Workspace Leg::generateWorkspace(void)
   Eigen::Vector3d origin_tip_position, target_tip_position;
   double distance_from_origin;
   int number_iterations;
-  double raw_number_iterations;
 
   // Iterate through all legs and move legs along search bearings to kinematic limits
   while (true)
@@ -367,7 +388,6 @@ Workspace Leg::generateWorkspace(void)
       if (!found_lower_limit || !found_upper_limit)
       {
         number_iterations = roundToInt(MAX_WORKSPACE_RADIUS / MAX_POSITION_DELTA);
-        raw_number_iterations = MAX_WORKSPACE_RADIUS / MAX_POSITION_DELTA;
         origin_tip_position = identity_tip_position;
         Eigen::Vector3d search_limit =
             (found_lower_limit ? MAX_WORKSPACE_RADIUS : -MAX_WORKSPACE_RADIUS) * Eigen::Vector3d::UnitZ();
@@ -379,7 +399,6 @@ Workspace Leg::generateWorkspace(void)
         number_iterations = roundToInt(search_height_delta / MAX_POSITION_DELTA);
         // Removes chance for zero iterations
         number_iterations = std::max(1, number_iterations);
-        raw_number_iterations = search_height_delta / MAX_POSITION_DELTA;
         origin_tip_position = current_tip_pose_.position_;
         target_tip_position = identity_tip_position;
       }
@@ -470,7 +489,6 @@ Workspace Leg::generateWorkspace(void)
         else
         {
           workspace_generation_complete = true;
-          ;
         }
       }
     }
@@ -684,9 +702,9 @@ void Leg::calculateTipForce(void)
 
   // Low pass filter and force gain applied to calculated raw tip force
   double s = 0.15; // Smoothing Factor
-  tip_force_calculated_[0] = s * raw_tip_force[0] * params_.force_gain.current_value + (1 - s) * tip_force_calculated_[0];
-  tip_force_calculated_[1] = s * raw_tip_force[1] * params_.force_gain.current_value + (1 - s) * tip_force_calculated_[1];
-  tip_force_calculated_[2] = s * raw_tip_force[2] * params_.force_gain.current_value + (1 - s) * tip_force_calculated_[2];
+  tip_force_calculated_[0] = s*raw_tip_force[0]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[0];
+  tip_force_calculated_[1] = s*raw_tip_force[1]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[1];
+  tip_force_calculated_[2] = s*raw_tip_force[2]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[2];
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -973,7 +991,14 @@ Pose Leg::applyFK(const bool &set_current, const bool &use_actual)
 
 Link::Link(std::shared_ptr<Leg> leg, std::shared_ptr<Joint> actuating_joint,
            const int &id_number, const Parameters &params)
-    : parent_leg_(leg), actuating_joint_(actuating_joint), id_number_(id_number), id_name_(leg->getIDName() + "_" + params.link_id.data[id_number_] + "_link"), dh_parameter_r_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("r")), dh_parameter_theta_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("theta")), dh_parameter_d_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("d")), dh_parameter_alpha_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("alpha"))
+    : parent_leg_(leg)
+    , actuating_joint_(actuating_joint)
+    , id_number_(id_number)
+    , id_name_(leg->getIDName() + "_" + params.link_id.data[id_number_] + "_link")
+    , dh_parameter_r_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("r"))
+    , dh_parameter_theta_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("theta"))
+    , dh_parameter_d_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("d"))
+    , dh_parameter_alpha_(params.link_parameters[leg->getIDNumber()][id_number_].data.at("alpha"))
 {
   if (!params.link_parameters[leg->getIDNumber()][id_number_].initialised)
   {
@@ -985,15 +1010,29 @@ Link::Link(std::shared_ptr<Leg> leg, std::shared_ptr<Joint> actuating_joint,
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Link::Link(std::shared_ptr<Link> link)
-    : parent_leg_(link->parent_leg_), actuating_joint_(link->actuating_joint_), id_number_(link->id_number_), id_name_(link->id_name_), dh_parameter_r_(link->dh_parameter_r_), dh_parameter_theta_(link->dh_parameter_theta_), dh_parameter_d_(link->dh_parameter_d_), dh_parameter_alpha_(link->dh_parameter_alpha_)
+    : parent_leg_(link->parent_leg_)
+    , actuating_joint_(link->actuating_joint_)
+    , id_number_(link->id_number_)
+    , id_name_(link->id_name_)
+    , dh_parameter_r_(link->dh_parameter_r_)
+    , dh_parameter_theta_(link->dh_parameter_theta_)
+    , dh_parameter_d_(link->dh_parameter_d_)
+    , dh_parameter_alpha_(link->dh_parameter_alpha_)
 {
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Joint::Joint(std::shared_ptr<Leg> leg, std::shared_ptr<Link> reference_link,
              const int &id_number, const Parameters &params)
-    : parent_leg_(leg), reference_link_(reference_link), id_number_(id_number), id_name_(leg->getIDName() + "_" + params.joint_id.data[id_number_ - 1] + "_joint"), min_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("min")), max_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max")), unpacked_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("unpacked")), max_angular_speed_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max_vel"))
+    : parent_leg_(leg)
+    , reference_link_(reference_link)
+    , id_number_(id_number)
+    , id_name_(leg->getIDName() + "_" + params.joint_id.data[id_number_ - 1] + "_joint")
+    , min_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("min"))
+    , max_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max"))
+    , unpacked_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("unpacked"))
+    , max_angular_speed_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max_vel"))
 {
   default_position_ = clamped(0.0, min_position_, max_position_);
 
@@ -1035,7 +1074,15 @@ Joint::Joint(std::shared_ptr<Leg> leg, std::shared_ptr<Link> reference_link,
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 Joint::Joint(std::shared_ptr<Joint> joint)
-    : parent_leg_(joint->parent_leg_), reference_link_(joint->reference_link_), id_number_(joint->id_number_), id_name_(joint->id_name_), min_position_(joint->min_position_), max_position_(joint->max_position_), packed_positions_(joint->packed_positions_), unpacked_position_(joint->unpacked_position_), max_angular_speed_(joint->max_angular_speed_)
+    : parent_leg_(joint->parent_leg_)
+    , reference_link_(joint->reference_link_)
+    , id_number_(joint->id_number_)
+    , id_name_(joint->id_name_)
+    , min_position_(joint->min_position_)
+    , max_position_(joint->max_position_)
+    , packed_positions_(joint->packed_positions_)
+    , unpacked_position_(joint->unpacked_position_)
+    , max_angular_speed_(joint->max_angular_speed_)
 {
   current_transform_ = joint->current_transform_;
   identity_transform_ = joint->identity_transform_;