Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/…

…speed-control-based-on-imu

include/suiryoku/locomotion/node/locomotion_node.hpp

@@ -52,8 +52,11 @@ class LocomotionNode
 
   void update();
 
+  bool set_odometry;
+
 private:
   void publish_walking();
+  void publish_odometry();
 
   rclcpp::Node::SharedPtr node;
 

include/suiryoku/locomotion/process/locomotion.hpp

@@ -48,6 +48,7 @@ class Locomotion
   void move_forward(const keisan::Angle<double> & direction, float delta_sec);
   bool move_forward_to(const keisan::Point2 & target, float delta_sec);
 
+  bool rotate_to_target(const keisan::Angle<double> & direction, float delta_sec);
   bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only, float delta_sec);
 
   bool move_follow_head(float delta_sec);
@@ -66,7 +67,11 @@ class Locomotion
     float delta_sec);
   bool position_left_kick(const keisan::Angle<double> & direction, float delta_sec);
   bool position_right_kick(const keisan::Angle<double> & direction, float delta_sec);
+  bool position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan, 
+                                     const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, 
+                                     const keisan::Angle<double> & max_tilt, float delta_sec);
   bool position_kick_general(const keisan::Angle<double> & direction, float delta_sec);
+  bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, float delta_sec);
 
   bool is_time_to_follow();
   bool pivot_fulfilled();
@@ -81,14 +86,33 @@ class Locomotion
   std::function<void()> stop;
   std::function<void()> start;
 
+  std::string config_name;
+
 private:
+
   double move_min_x;
   double move_max_x;
   double move_max_y;
   double move_max_a;
 
+  double backward_max_x;
+  double backward_min_x;
+  double backward_max_a;
+
+  double rotate_max_a;
+  double rotate_max_delta_direction;
+
+  double follow_pan_ratio;
   double follow_max_x;
+  double follow_min_x;
   double follow_max_a;
+  double follow_l_a_offset;
+  double follow_r_a_offset;
+  bool follow_y_move;
+  double follow_max_ry;
+  double follow_min_ry;
+  double follow_max_ly;
+  double follow_min_ly;
   keisan::Angle<double> follow_min_tilt;
 
   double dribble_min_x;
@@ -98,8 +122,11 @@ class Locomotion
   double dribble_min_ry;
   double dribble_max_ry;
   double dribble_max_a;
+  double dribble_pan_comp;
 
   keisan::Angle<double> pivot_target_tilt;
+  double pivot_max_delta_direction;
+  double pivot_pan_range_ratio;
   double pivot_min_x;
   double pivot_max_x;
   double pivot_max_ly;
@@ -113,13 +140,18 @@ class Locomotion
   double position_min_ry;
   double position_max_ry;
   double position_max_a;
-  double position_min_delta_tilt;
-  double position_min_delta_pan;
-  double position_min_delta_pan_tilt;
-  double position_min_delta_direction;
-  double position_prev_delta_pan;
-  double position_prev_delta_tilt;
   double position_in_belief;
+  keisan::Angle<double> position_min_delta_tilt;
+  keisan::Angle<double> position_min_delta_pan;
+  keisan::Angle<double> position_min_delta_pan_tilt;
+  keisan::Angle<double> position_min_delta_direction;
+  keisan::Angle<double> position_prev_delta_pan;
+  keisan::Angle<double> position_prev_delta_tilt;
+  keisan::Angle<double> position_min_range_tilt;
+  keisan::Angle<double> position_max_range_tilt;
+  keisan::Angle<double> position_min_range_pan;
+  keisan::Angle<double> position_center_range_pan;
+  keisan::Angle<double> position_max_range_pan;
 
   double skew_max_x;
   double skew_max_a;

src/suiryoku/locomotion/node/locomotion_node.cpp

@@ -41,13 +41,13 @@ std::string LocomotionNode::get_node_prefix()
 
 LocomotionNode::LocomotionNode(
   rclcpp::Node::SharedPtr node, std::shared_ptr<Locomotion> locomotion)
-: locomotion(locomotion), robot(locomotion->get_robot()), walking_state(false)
+: locomotion(locomotion), robot(locomotion->get_robot()), walking_state(false), set_odometry(false)
 {
   set_walking_publisher = node->create_publisher<SetWalking>(
     aruku::WalkingNode::set_walking_topic(), 10);
 
   measurement_status_subscriber = node->create_subscription<MeasurementStatus>(
-    "/measurement/orientation", 10,
+    kansei::measurement::MeasurementNode::status_topic(), 10,
     [this](const MeasurementStatus::SharedPtr message) {
       this->robot->is_calibrated = message->is_calibrated;
       this->robot->orientation = keisan::make_degree(message->orientation.yaw);
@@ -83,6 +83,9 @@ LocomotionNode::LocomotionNode(
 void LocomotionNode::update()
 {
   publish_walking();
+  if (set_odometry) {
+    publish_odometry();
+  }
 }
 
 void LocomotionNode::publish_walking()
@@ -98,4 +101,15 @@ void LocomotionNode::publish_walking()
   set_walking_publisher->publish(walking_msg);
 }
 
+void LocomotionNode::publish_odometry()
+{
+  auto odometry_msg = Point2();
+
+  odometry_msg.x = robot->position.x;
+  odometry_msg.y = robot->position.y;
+
+  set_odometry_publisher->publish(odometry_msg);
+  set_odometry = false;
+}
+
 }  // namespace suiryoku