Merge remote-tracking branch 'origin/master' into feature/joint_offset_param

Author: benjamin-tam

include/syropod_highlevel_controller/state_controller.h

@@ -217,14 +217,14 @@ class StateController
   /// @param[in] input The Point geometry message provided by the topic "syropod_remote/secondary_tip_velocity"
   void secondaryTipVelocityInputCallback(const geometry_msgs::Point &input);
 
-  /// 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)
+  /// Callback for the input manual tip pose (in cartesian space) for the primary selected leg. 
+  /// @param[in] input The Pose geometry message provided by the subscribed topic
+  /// "/syropod_manipulation/primary_tip_pose"
   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)
+  /// Callback for the input manual tip pose (in cartesian space) for the secondary selected leg. 
+  /// @param[in] input The Pose geometry message provided by the subscribed topic
+  /// "/syropod_manipulation/secondary_tip_pose"
   void  secondaryTipPoseInputCallback(const geometry_msgs::Pose &msg);
 
   /// Callback handling the desired parameter selection and sending state messages to user interface.
@@ -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 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 primary_tip_pose_subscriber_;   ///< Subscriber for topic /syropod_manipulation/primary_tip_pose
+  ros::Subscriber secondary_tip_pose_subscriber_; ///< Subscriber for topic /syropod_manipulation/secondary_tip_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 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
+  Pose primary_pose_input_;   ///< Input for the desired pose of primary leg tip 
+  Pose secondary_pose_input_; ///< Input for the desired pose of secondary the leg tip
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

readme.md

@@ -98,14 +98,14 @@ For information on parameters see readme in config folder.
     * Type: std_msgs::Int8
 
 ### Syropod Manipulation:
-* AR Leg Tip Position:
-    * Description: The desired position of leg AR in Cartesian space.   
-    * Topic: */syropod_manipulation/AR/Pose*
+* Primary Leg Tip Pose:
+    * Description: The desired pose for the leg selected for primary manipulation within Cartesian space.   
+    * Topic: */syropod_manipulation/primary_tip_pose*
     * Type:  geometry_msgs::Pose
 
-* AL Leg Tip Position:
-    * Description: The desired position of leg AL in Cartesian space.   
-    * Topic: */syropod_manipulation/AL/Pose*
+* Secondary Leg Tip Pose:
+    * Description: The desired pose for the leg selected for secondary manipulation within Cartesian space.   
+    * Topic: */syropod_manipulation/secondary_tip_pose*
     * Type:  geometry_msgs::Pose
 
 ### Motor and Sensor Inputs

src/state_controller.cpp

@@ -63,10 +63,11 @@ StateController::StateController(void)
                                                 &StateController::parameterSelectionCallback, this);
   parameter_adjustment_subscriber_ = n.subscribe("syropod_remote/parameter_adjustment", 1,
                                                  &StateController::parameterAdjustCallback, this);
+
   // Hexapod Leg Manipulation topic subscriptions
-  primary_tip_pose_subscriber_ = n.subscribe("/syropod_manipulation/AR/Pose", 1,
+  primary_tip_pose_subscriber_ = n.subscribe("/syropod_manipulation/primary_tip_pose", 1,
                                              &StateController::primaryTipPoseInputCallback, this);
-  secondary_tip_pose_subscriber_ = n.subscribe("/syropod_manipulation/AL/Pose", 1,
+  secondary_tip_pose_subscriber_ = n.subscribe("/syropod_manipulation/secondary_tip_pose", 1,
                                                &StateController::secondaryTipPoseInputCallback, this);
 
   // Planner subscription/publisher

src/walk_controller.cpp

@@ -734,7 +734,6 @@ void WalkController::updateManual(const int &primary_leg_selection_ID, const Pos
       {
         if (params_.leg_manipulation_mode.data == "tip_control")
         {
-          
           // TODO add orientation control
           // leg_stepper->setCurrentTipPose(Pose(tip_position_input, tip_rotation_input));
           leg_stepper->setCurrentTipPose(Pose(tip_position_input, UNDEFINED_ROTATION));
@@ -1095,7 +1094,7 @@ void LegStepper::updateTipPosition(void)
           target_tip_pose_.position_ += getProjection(difference, walk_plane_normal_);
         }
         // Shift target toward step surface by defined distance and rely on step surface contact detection (REACTIVE)
-        else
+        else 
         {
           target_tip_pose_.position_ -= walker_->getStepDepth() * Eigen::Vector3d::UnitZ();
         }