complete free gait rviz plugin, able to use rqt action send action to…

… move robot in rviz

free_gait_action_loader/src/free_gait_action_loader/action_handling.py

@@ -69,6 +69,7 @@ def _initialize(self, package_path, parameters):
             self.name = parameters['name']
         if 'file' in parameters:
             self.file = abspath(join(package_path, parameters['file']))
+            rospy.loginfo(self.file)
         if 'type' in parameters:
             self.type = ActionType.from_text(parameters['type'])
         if 'description' in parameters:
@@ -95,11 +96,13 @@ def update(self):
         self.actions = []
         rospack = rospkg.RosPack()
         packages = rospack.get_depends_on(self.name, implicit=False)
-        rospy.loginfo(packages)
+        rospy.loginfo(rospack)
         for package in packages:
             rospy.loginfo(package)
             manifest = rospack.get_manifest(package)
+            rospy.loginfo(manifest.exports)
             file_path = manifest.get_export(self.name, 'actions')
+            rospy.loginfo(file_path)
             if not file_path:
                 continue
             elif len(file_path) != 1:
@@ -114,7 +117,9 @@ def update(self):
 
                 package_path = rospack.get_path(package)
                 parameters = load_file(file_path)
+                # rospy.loginfo(parameters[0][0]['actions'])
                 for action_parameters in parameters[0][0]['actions']:
+                    rospy.loginfo(action_parameters)
                     entry = ActionEntry(package_path, action_parameters['action'])
                     self.actions.append(entry)
 

free_gait_core/CMakeLists.txt

@@ -94,6 +94,7 @@ add_library(${PROJECT_NAME}
    src/executor/StateBatch.cpp
    src/executor/StateBatchComputer.cpp
    src/executor/AdapterBase.cpp
+#   src/executor/AdapterDummy.cpp
    src/pose_optimization/PoseOptimizationBase.cpp
    src/pose_optimization/PoseConstraintsChecker.cpp
    src/pose_optimization/PoseOptimizationGeometric.cpp

free_gait_core/package.xml

@@ -19,4 +19,9 @@
   <depend>robot_utils</depend>
   <depend>std_utils</depend>
   <depend>message_logger</depend>
+
+  <!-- <export>
+    <free_gait_core plugin="${prefix}/plugin_description.xml"/>
+  </export> -->
+
 </package>

free_gait_core/plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="lib/libfree_gait_core">
+  <class name="free_gait_core/AdapterDummy"
+         type="free_gait::AdapterDummy"
+         base_class_type="free_gait::AdapterBase">
+    <description>Display to preview Free Gait actions.</description>
+  </class>
+</library>

free_gait_core/src/executor/BatchExecutor.cpp

@@ -73,7 +73,9 @@ void BatchExecutor::processInThread()
   while (!executor_.getQueue().empty() && !requestForCancelling_) {
     executor_.advance(timeStep_);
     time += timeStep_;
+//    std::cout<<"Updated Joint Positions: "<<std::endl<<executor_.getState().getJointPositions()<<std::endl;
     stateBatch_.addState(time, executor_.getState());
+//    std::cout<<"Updated Joint Positions: "<<std::endl<<stateBatch_.getState(time).getJointPositions()<<std::endl;
   }
   requestForCancelling_ = false;
   isProcessing_ = false;

free_gait_core/src/executor/Executor.cpp

@@ -74,7 +74,7 @@ bool Executor::advance(double dt, bool skipStateMeasurmentUpdate)
   }
 
   // Advance queue.
-  std::cout<<"Current Step Time : "<<queue_.getCurrentStep().getTime()<<std::endl;
+//  std::cout<<"Current Step Time : "<<queue_.getCurrentStep().getTime()<<std::endl;
   if (!queue_.advance(dt)) return false;//Update a step cycle time
   if (!adapter_.updateExtrasBefore(queue_, state_)) return false;
 
@@ -109,7 +109,7 @@ bool Executor::advance(double dt, bool skipStateMeasurmentUpdate)
     stream << "Switched step to:" << std::endl << queue_.getCurrentStep();
     addToFeedback(stream.str());
   }
-  std::cout<<"Executor update dt finished"<<std::endl;
+//  std::cout<<"===========================Executor update dt started========================"<<std::endl;
   if (!writeIgnoreContact()) return false;
   if (!writeIgnoreForPoseAdaptation()) return false;
   if (!writeSupportLegs()) return false;
@@ -118,8 +118,10 @@ bool Executor::advance(double dt, bool skipStateMeasurmentUpdate)
   if (!writeTorsoMotion()) return false;
   if (!writeStepId()) return false;
   if (!adapter_.updateExtrasAfter(queue_, state_)) return false;// TODO(Shunyao): update state to ex robot or sim
-  std::cout << state_ << std::endl;
-
+//  std::cout << state_ << std::endl;
+//  std::cout<<"===========================Executor update dt finished======================="<<std::endl;
+//  std::cout << state_.getPositionWorldToBaseInWorldFrame()<< std::endl;
+//  std::cout<<"Updated Joint Positions: "<<std::endl<<state_.getJointPositions()<<std::endl;
   return true;
 }
 
@@ -435,7 +437,7 @@ bool Executor::writeTorsoMotion()
                                                     baseMotion.evaluatePose(time));
     state_.setPositionWorldToBaseInWorldFrame(poseInWorldFrame.getPosition());
     state_.setOrientationBaseToWorld(poseInWorldFrame.getRotation());
-    std::cout<<"pose in world frame : "<<poseInWorldFrame.getPosition()<<std::endl;
+//    std::cout<<"pose in world frame : "<<poseInWorldFrame.getPosition()<<std::endl;
   }
   if (controlSetup[ControlLevel::Velocity]) {
     const std::string& frameId = baseMotion.getFrameId(ControlLevel::Velocity);

free_gait_core/src/executor/State.cpp

@@ -33,6 +33,7 @@ void State::initialize(const std::vector<LimbEnum>& limbs, const std::vector<Bra
   for (const auto& branch : branches) {
     setEmptyControlSetup(branch);
   }
+  QuadrupedState::Initialize();
 }
 
 bool State::getRobotExecutionStatus() const

free_gait_core/src/executor/StateBatchComputer.cpp

@@ -67,6 +67,7 @@ void StateBatchComputer::computeEndEffectorTrajectories(StateBatch& stateBatch)
   for (const auto& state : stateBatch.getStates()) {
     adapter_.setInternalDataFromState(state.second);
     size_t i = 0;
+//    std::cout<<state.second.getJointPositionsToReach()<<std::endl;
     for (const auto& limb : adapter_.getLimbs()) {
       const Position position = adapter_.getPositionWorldToFootInWorldFrame(limb);
       stateBatch.endEffectorPositions_[i++][state.first] = position;

free_gait_ros/CMakeLists.txt

@@ -75,16 +75,27 @@ add_library(${PROJECT_NAME}
    src/FreeGaitActionClient.cpp
    src/AdapterRos.cpp
    src/AdapterRosInterfaceBase.cpp
+   #src/AdapterRosInterfaceDummy.cpp
    src/StateRosPublisher.cpp
    src/RosVisualization.cpp
 )
-
+add_library(${PROJECT_NAME}_plugin
+  src/AdapterRosInterfaceDummy.cpp
+  src/AdapterDummy.cpp
+  )
+target_link_libraries(${PROJECT_NAME}_plugin
+  ${catkin_LIBRARIES}
+)
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
-add_executable(steps_test test/test.cpp test/AdapterDummy.cpp)
+add_executable(steps_test test/test.cpp) #test/AdapterDummy.cpp)
 target_link_libraries(steps_test ${PROJECT_NAME}
   ${catkin_LIBRARIES})
+add_executable(action_server_test_node test/action_server_test.cpp)
+target_link_libraries(action_server_test_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES})
 #############
 ## Testing ##
 #############

free_gait_ros/include/free_gait_ros/AdapterDummy.hpp

@@ -0,0 +1,104 @@
+/*
+ * AdapterDummy.hpp
+ *
+ *  Created on: Mar 23, 2017
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich
+ */
+
+#pragma once
+
+#include "free_gait_core/executor/AdapterBase.hpp"
+#include "free_gait_core/free_gait_core.hpp"
+#include <memory>
+
+namespace free_gait {
+
+class AdapterDummy : public AdapterBase
+{
+ public:
+  AdapterDummy();
+  virtual ~AdapterDummy();
+
+  //! Copying data from real robot to free gait state.
+  virtual bool resetExtrasWithRobot(const StepQueue& stepQueue, State& state);
+  virtual bool updateExtrasBefore(const StepQueue& stepQueue, State& state);
+  virtual bool updateExtrasAfter(const StepQueue& stepQueue, State& state);
+
+  //! State independent functions.
+  virtual const std::string& getWorldFrameId() const;
+  virtual const std::string& getBaseFrameId() const;
+  virtual const std::vector<LimbEnum>& getLimbs() const;
+  virtual const std::vector<BranchEnum>& getBranches() const;
+  virtual LimbEnum getLimbEnumFromLimbString(const std::string& limb) const;
+  virtual std::string getLimbStringFromLimbEnum(const LimbEnum& limb) const;
+  virtual std::string getBaseString() const;
+  virtual JointNodeEnum getJointNodeEnumFromJointNodeString(const std::string& jointNode) const;
+  virtual std::string getJointNodeStringFromJointNodeEnum(const JointNodeEnum& jointNode) const;
+  virtual bool getLimbJointPositionsFromPositionBaseToFootInBaseFrame(
+      const Position& positionBaseToFootInBaseFrame, const LimbEnum& limb,
+      JointPositionsLeg& jointPositions) const;
+  virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb,
+                                                    const JointPositionsLeg& jointPositions) const;
+  virtual Position getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const;
+
+  //! Reading state of real robot.
+  virtual bool isExecutionOk() const;
+  virtual bool isLegGrounded(const LimbEnum& limb) const;
+  virtual JointPositionsLeg getJointPositionsForLimb(const LimbEnum& limb) const;
+  virtual JointPositions getAllJointPositions() const;
+  virtual JointVelocitiesLeg getJointVelocitiesForLimb(const LimbEnum& limb) const;
+  virtual JointVelocities getAllJointVelocities() const;
+  virtual JointAccelerationsLeg getJointAccelerationsForLimb(const LimbEnum& limb) const;
+  virtual JointAccelerations getAllJointAccelerations() const;
+  virtual JointEffortsLeg getJointEffortsForLimb(const LimbEnum& limb) const;
+  virtual JointEfforts getAllJointEfforts() const;
+  virtual Position getPositionWorldToBaseInWorldFrame() const;
+  virtual RotationQuaternion getOrientationBaseToWorld() const;
+  virtual LinearVelocity getLinearVelocityBaseInWorldFrame() const;
+  virtual LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const;
+  virtual LinearAcceleration getLinearAccelerationBaseInWorldFrame() const;
+  virtual AngularAcceleration getAngularAccelerationBaseInBaseFrame() const;
+  virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb) const;
+  virtual Position getPositionWorldToFootInWorldFrame(const LimbEnum& limb) const;
+  virtual Position getCenterOfMassInWorldFrame() const;
+
+  /*!
+   * Transform is frameId to world => C_IF.
+   * @param frameId
+   * @return
+   */
+  virtual void getAvailableFrameTransforms(std::vector<std::string>& frameTransforms) const;
+  virtual Pose getFrameTransform(const std::string& frameId) const;
+  virtual ControlSetup getControlSetup(const BranchEnum& branch) const;
+  virtual ControlSetup getControlSetup(const LimbEnum& limb) const;
+
+  //! Depending on state of real robot.
+  virtual JointVelocitiesLeg getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame(
+      const LimbEnum& limb, const LinearVelocity& endEffectorLinearVelocityInWorldFrame) const;
+  virtual LinearVelocity getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb,
+                                                                         const JointVelocitiesLeg& jointVelocities,
+                                                                         const std::string& frameId) const;
+  virtual JointAccelerationsLeg getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame(
+      const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const;
+
+  //! Hook to write data to internal robot representation from state.
+//  virtual bool setInternalDataFromState(const State& state) const;
+  virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true,
+                                          bool updateVelocity = true, bool updateAcceleration = false) const;
+
+  virtual void createCopyOfState() const;
+  virtual void resetToCopyOfState() const;
+
+  const State& getState() const;
+
+ private:
+  std::unique_ptr<State> state_;
+  std::vector<LimbEnum> limbs_;
+  std::vector<BranchEnum> branches_;
+  const std::string worldFrameId_;
+  const std::string baseFrameId_;
+  Stance footholdsInSupport_;
+};
+
+} /* namespace free_gait */

free_gait_ros/include/free_gait_ros/AdapterRos.hpp

@@ -12,6 +12,7 @@
 #include <free_gait_ros/AdapterRosInterfaceBase.hpp>
 #include <ros/node_handle.h>
 #include <pluginlib/class_loader.h>
+#include "pluginlib_tutorials/polygon_base.h"
 
 // STD
 #include <memory>
@@ -39,6 +40,8 @@ class AdapterRos
 
  private:
   ros::NodeHandle& nodeHandle_;
+  pluginlib::ClassLoader<polygon_base::RegularPolygon> tutor_loader_;
+  std::unique_ptr<polygon_base::RegularPolygon> tutor_;
   pluginlib::ClassLoader<AdapterBase> adapterLoader_;
   std::unique_ptr<AdapterBase> adapter_;
   pluginlib::ClassLoader<AdapterRosInterfaceBase> adapterRosInterfaceLoader_;

free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp

@@ -36,13 +36,13 @@ class AdapterRosInterfaceBase
   virtual bool initializeAdapter(AdapterBase& adapter) const = 0;
   virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const = 0;
 
-  void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState);
+//  void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState);
 
  protected:
   ros::NodeHandle* nodeHandle_;
   std::string robotDescriptionUrdfString_;
-  ros::Subscriber joint_states_sub_;
-  JointPositions all_joint_positions_;
+//  ros::Subscriber joint_states_sub_;
+//  JointPositions all_joint_positions_;
 
 };
 

free_gait_ros/include/free_gait_ros/AdapterRosInterfaceDummy.hpp

@@ -0,0 +1,40 @@
+/*
+ *  filename.hpp
+ *  Descriotion:
+ *
+ *  Created on: Dec 26, 2018
+ *  Author: Shunyao Wang
+ *  Institute: Harbin Institute of Technology, Shenzhen
+ */
+
+#pragma once
+#include "moveit_msgs/RobotState.h"
+#include "free_gait_ros/free_gait_ros.hpp"
+
+namespace free_gait {
+
+class AdapterRosInterfaceDummy : public AdapterRosInterfaceBase
+{
+public:
+  AdapterRosInterfaceDummy();
+  ~AdapterRosInterfaceDummy();
+
+  virtual bool subscribeToRobotState(const std::string& robotStateTopic);
+  virtual void unsubscribeFromRobotState();
+  virtual const std::string getRobotStateMessageType();
+  virtual bool isReady() const;
+
+  //! Update adapter.
+  virtual bool initializeAdapter(AdapterBase& adapter) const;
+  virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const;
+
+  void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState);
+
+
+private:
+  ros::Subscriber joint_states_sub_;
+  JointPositions all_joint_positions_;
+
+};
+
+} // namespace

free_gait_ros/include/free_gait_ros/free_gait_ros.hpp

@@ -17,3 +17,4 @@
 #include "free_gait_ros/StepFrameConverter.hpp"
 #include "free_gait_ros/StateRosPublisher.hpp"
 #include "free_gait_ros/RosVisualization.hpp"
+#include "free_gait_ros/AdapterDummy.hpp"

free_gait_ros/launch/test.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <param name="/free_gait/adapter_plugin/preview" value="free_gait::AdapterDummy" />
+  <param name="/free_gait/adapter_ros_interface_plugin" value="free_gait::AdapterRosInterfaceDummy" />
+  <node pkg="free_gait_ros" type="steps_test" name="steps_test_node"
+        output="screen">
+  </node>
+</launch>

free_gait_ros/package.xml

@@ -22,4 +22,8 @@
   <depend>robot_state_publisher</depend>
   <depend>kdl_parser</depend>
   <depend>eigen</depend>
+
+  <export>
+    <free_gait_ros plugin="${prefix}/plugin_description.xml"/>
+  </export>
 </package>

free_gait_ros/plugin_description.xml

@@ -0,0 +1,12 @@
+<library path="lib/libfree_gait_ros_plugin">
+  <class name="free_gait_ros/AdapterRosInterfaceDummy"
+         type="free_gait::AdapterRosInterfaceDummy"
+         base_class_type="free_gait::AdapterRosInterfaceBase">
+    <description>Display to preview Free Gait actions.</description>
+  </class>
+  <class name="free_gait_ros/AdapterDummy"
+         type="free_gait::AdapterDummy"
+         base_class_type="free_gait::AdapterBase">
+    <description>Display to preview Free Gait actions.</description>
+  </class>
+</library>