From 675825ec5fa92301f561b92bfe36db745ea14683 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sun, 24 Nov 2024 12:26:18 -0600 Subject: [PATCH 1/4] Updated ros messages for Continuous Hiking --- .../msg/ContinuousHikingCommandMessage_.idl | 72 +++ .../msg/ContinuousWalkingCommandMessage_.idl | 63 --- .../dds/ContinuousHikingCommandMessage.java | 424 ++++++++++++++++++ ...inuousHikingCommandMessagePubSubType.java} | 154 ++++--- .../dds/ContinuousWalkingCommandMessage.java | 367 --------------- .../msg/ContinuousHikingCommandMessage.msg | 39 ++ .../msg/ContinuousWalkingCommandMessage.msg | 32 -- .../msg/ContinuousHikingCommandMessage.msg | 42 ++ .../msg/ContinuousWalkingCommandMessage.msg | 35 -- 9 files changed, 665 insertions(+), 563 deletions(-) create mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl delete mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousWalkingCommandMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java rename ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/{ContinuousWalkingCommandMessagePubSubType.java => ContinuousHikingCommandMessagePubSubType.java} (61%) delete mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessage.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousHikingCommandMessage.msg delete mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousHikingCommandMessage.msg delete mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl new file mode 100644 index 00000000000..c0219ffa155 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousHikingCommandMessage_.idl @@ -0,0 +1,72 @@ +#ifndef __behavior_msgs__msg__ContinuousHikingCommandMessage__idl__ +#define __behavior_msgs__msg__ContinuousHikingCommandMessage__idl__ + +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::ContinuousHikingCommandMessage_") + struct ContinuousHikingCommandMessage + { + /** + * Flag to start Continuous Hiking + */ + boolean enable_continuous_hiking; + /** + * Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever + */ + unsigned long steps_before_safety_stop; + /** + * Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick + */ + boolean walk_forwards; + /** + * Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. + * Setting this to true tells the robot to step on the final goal positions. + */ + boolean square_up_to_goal; + /** + * Flag to enable/disable planning with A* planner. This is the default planner to use + */ + boolean use_astar_footstep_planner; + /** + * Flag to enable/disable planning with monte-carlo footstep planner + */ + boolean use_monte_carlo_footstep_planner; + /** + * Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time + */ + boolean use_previous_plan_as_reference; + /** + * Flag to enable/disable using monte-carlo plan as reference + */ + boolean use_monte_carlo_plan_as_reference; + /** + * Walk with the Joystick Controller + */ + boolean use_joystick_controller; + /** + * Forward joystick value + */ + double forward_value; + /** + * Backward walking from Joystick Controller + */ + boolean walk_backwards; + /** + * Lateral joystick value + */ + double lateral_value; + /** + * Turning joystick value + */ + double turning_value; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousWalkingCommandMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousWalkingCommandMessage_.idl deleted file mode 100644 index ae380bb92df..00000000000 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/ContinuousWalkingCommandMessage_.idl +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef __behavior_msgs__msg__ContinuousWalkingCommandMessage__idl__ -#define __behavior_msgs__msg__ContinuousWalkingCommandMessage__idl__ - -module behavior_msgs -{ - module msg - { - module dds - { - - @TypeCode(type="behavior_msgs::msg::dds_::ContinuousWalkingCommandMessage_") - struct ContinuousWalkingCommandMessage - { - /** - * flag to enable/disable continuous hiking state machine with keyboard - */ - boolean enable_continuous_hiking_with_keyboard; - /** - * flag to enable/disable continuous hiking state machine with joystick controller - */ - boolean enable_continuous_hiking_with_joystick_controller; - /** - * flag to walk backwards with the continuous hiking state machine - */ - boolean walk_backwards; - /** - * forward joystick value - */ - double forward_value; - /** - * lateral joystick value - */ - double lateral_value; - /** - * turning joystick value - */ - double turning_value; - /** - * flag to enable/disable hybrid planning - */ - boolean use_hybrid_planner; - /** - * flag to enable/disable planning with astar planner - */ - boolean use_astar_footstep_planner; - /** - * flag to enable/disable planning with monte-carlo footstep planner - */ - boolean use_monte_carlo_footstep_planner; - /** - * flag to enable/disable using previous plan as reference - */ - boolean use_previous_plan_as_reference; - /** - * flag to enable/disable using monte-carlo plan as reference - */ - boolean use_monte_carlo_plan_as_reference; - }; - }; - }; -}; - -#endif diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java new file mode 100644 index 00000000000..5ac403ee581 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessage.java @@ -0,0 +1,424 @@ +package behavior_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +public class ContinuousHikingCommandMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Flag to start Continuous Hiking + */ + public boolean enable_continuous_hiking_; + /** + * Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever + */ + public long steps_before_safety_stop_; + /** + * Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick + */ + public boolean walk_forwards_; + /** + * Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. + * Setting this to true tells the robot to step on the final goal positions. + */ + public boolean square_up_to_goal_; + /** + * Flag to enable/disable planning with A* planner. This is the default planner to use + */ + public boolean use_astar_footstep_planner_; + /** + * Flag to enable/disable planning with monte-carlo footstep planner + */ + public boolean use_monte_carlo_footstep_planner_; + /** + * Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time + */ + public boolean use_previous_plan_as_reference_; + /** + * Flag to enable/disable using monte-carlo plan as reference + */ + public boolean use_monte_carlo_plan_as_reference_; + /** + * Walk with the Joystick Controller + */ + public boolean use_joystick_controller_; + /** + * Forward joystick value + */ + public double forward_value_; + /** + * Backward walking from Joystick Controller + */ + public boolean walk_backwards_; + /** + * Lateral joystick value + */ + public double lateral_value_; + /** + * Turning joystick value + */ + public double turning_value_; + + public ContinuousHikingCommandMessage() + { + } + + public ContinuousHikingCommandMessage(ContinuousHikingCommandMessage other) + { + this(); + set(other); + } + + public void set(ContinuousHikingCommandMessage other) + { + enable_continuous_hiking_ = other.enable_continuous_hiking_; + + steps_before_safety_stop_ = other.steps_before_safety_stop_; + + walk_forwards_ = other.walk_forwards_; + + square_up_to_goal_ = other.square_up_to_goal_; + + use_astar_footstep_planner_ = other.use_astar_footstep_planner_; + + use_monte_carlo_footstep_planner_ = other.use_monte_carlo_footstep_planner_; + + use_previous_plan_as_reference_ = other.use_previous_plan_as_reference_; + + use_monte_carlo_plan_as_reference_ = other.use_monte_carlo_plan_as_reference_; + + use_joystick_controller_ = other.use_joystick_controller_; + + forward_value_ = other.forward_value_; + + walk_backwards_ = other.walk_backwards_; + + lateral_value_ = other.lateral_value_; + + turning_value_ = other.turning_value_; + + } + + /** + * Flag to start Continuous Hiking + */ + public void setEnableContinuousHiking(boolean enable_continuous_hiking) + { + enable_continuous_hiking_ = enable_continuous_hiking; + } + /** + * Flag to start Continuous Hiking + */ + public boolean getEnableContinuousHiking() + { + return enable_continuous_hiking_; + } + + /** + * Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever + */ + public void setStepsBeforeSafetyStop(long steps_before_safety_stop) + { + steps_before_safety_stop_ = steps_before_safety_stop; + } + /** + * Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever + */ + public long getStepsBeforeSafetyStop() + { + return steps_before_safety_stop_; + } + + /** + * Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick + */ + public void setWalkForwards(boolean walk_forwards) + { + walk_forwards_ = walk_forwards; + } + /** + * Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick + */ + public boolean getWalkForwards() + { + return walk_forwards_; + } + + /** + * Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. + * Setting this to true tells the robot to step on the final goal positions. + */ + public void setSquareUpToGoal(boolean square_up_to_goal) + { + square_up_to_goal_ = square_up_to_goal; + } + /** + * Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. + * Setting this to true tells the robot to step on the final goal positions. + */ + public boolean getSquareUpToGoal() + { + return square_up_to_goal_; + } + + /** + * Flag to enable/disable planning with A* planner. This is the default planner to use + */ + public void setUseAstarFootstepPlanner(boolean use_astar_footstep_planner) + { + use_astar_footstep_planner_ = use_astar_footstep_planner; + } + /** + * Flag to enable/disable planning with A* planner. This is the default planner to use + */ + public boolean getUseAstarFootstepPlanner() + { + return use_astar_footstep_planner_; + } + + /** + * Flag to enable/disable planning with monte-carlo footstep planner + */ + public void setUseMonteCarloFootstepPlanner(boolean use_monte_carlo_footstep_planner) + { + use_monte_carlo_footstep_planner_ = use_monte_carlo_footstep_planner; + } + /** + * Flag to enable/disable planning with monte-carlo footstep planner + */ + public boolean getUseMonteCarloFootstepPlanner() + { + return use_monte_carlo_footstep_planner_; + } + + /** + * Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time + */ + public void setUsePreviousPlanAsReference(boolean use_previous_plan_as_reference) + { + use_previous_plan_as_reference_ = use_previous_plan_as_reference; + } + /** + * Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time + */ + public boolean getUsePreviousPlanAsReference() + { + return use_previous_plan_as_reference_; + } + + /** + * Flag to enable/disable using monte-carlo plan as reference + */ + public void setUseMonteCarloPlanAsReference(boolean use_monte_carlo_plan_as_reference) + { + use_monte_carlo_plan_as_reference_ = use_monte_carlo_plan_as_reference; + } + /** + * Flag to enable/disable using monte-carlo plan as reference + */ + public boolean getUseMonteCarloPlanAsReference() + { + return use_monte_carlo_plan_as_reference_; + } + + /** + * Walk with the Joystick Controller + */ + public void setUseJoystickController(boolean use_joystick_controller) + { + use_joystick_controller_ = use_joystick_controller; + } + /** + * Walk with the Joystick Controller + */ + public boolean getUseJoystickController() + { + return use_joystick_controller_; + } + + /** + * Forward joystick value + */ + public void setForwardValue(double forward_value) + { + forward_value_ = forward_value; + } + /** + * Forward joystick value + */ + public double getForwardValue() + { + return forward_value_; + } + + /** + * Backward walking from Joystick Controller + */ + public void setWalkBackwards(boolean walk_backwards) + { + walk_backwards_ = walk_backwards; + } + /** + * Backward walking from Joystick Controller + */ + public boolean getWalkBackwards() + { + return walk_backwards_; + } + + /** + * Lateral joystick value + */ + public void setLateralValue(double lateral_value) + { + lateral_value_ = lateral_value; + } + /** + * Lateral joystick value + */ + public double getLateralValue() + { + return lateral_value_; + } + + /** + * Turning joystick value + */ + public void setTurningValue(double turning_value) + { + turning_value_ = turning_value; + } + /** + * Turning joystick value + */ + public double getTurningValue() + { + return turning_value_; + } + + + public static Supplier getPubSubType() + { + return ContinuousHikingCommandMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return ContinuousHikingCommandMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(ContinuousHikingCommandMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_continuous_hiking_, other.enable_continuous_hiking_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.steps_before_safety_stop_, other.steps_before_safety_stop_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.walk_forwards_, other.walk_forwards_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.square_up_to_goal_, other.square_up_to_goal_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_astar_footstep_planner_, other.use_astar_footstep_planner_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_monte_carlo_footstep_planner_, other.use_monte_carlo_footstep_planner_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_previous_plan_as_reference_, other.use_previous_plan_as_reference_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_monte_carlo_plan_as_reference_, other.use_monte_carlo_plan_as_reference_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_joystick_controller_, other.use_joystick_controller_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.forward_value_, other.forward_value_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.walk_backwards_, other.walk_backwards_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.lateral_value_, other.lateral_value_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.turning_value_, other.turning_value_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof ContinuousHikingCommandMessage)) return false; + + ContinuousHikingCommandMessage otherMyClass = (ContinuousHikingCommandMessage) other; + + if(this.enable_continuous_hiking_ != otherMyClass.enable_continuous_hiking_) return false; + + if(this.steps_before_safety_stop_ != otherMyClass.steps_before_safety_stop_) return false; + + if(this.walk_forwards_ != otherMyClass.walk_forwards_) return false; + + if(this.square_up_to_goal_ != otherMyClass.square_up_to_goal_) return false; + + if(this.use_astar_footstep_planner_ != otherMyClass.use_astar_footstep_planner_) return false; + + if(this.use_monte_carlo_footstep_planner_ != otherMyClass.use_monte_carlo_footstep_planner_) return false; + + if(this.use_previous_plan_as_reference_ != otherMyClass.use_previous_plan_as_reference_) return false; + + if(this.use_monte_carlo_plan_as_reference_ != otherMyClass.use_monte_carlo_plan_as_reference_) return false; + + if(this.use_joystick_controller_ != otherMyClass.use_joystick_controller_) return false; + + if(this.forward_value_ != otherMyClass.forward_value_) return false; + + if(this.walk_backwards_ != otherMyClass.walk_backwards_) return false; + + if(this.lateral_value_ != otherMyClass.lateral_value_) return false; + + if(this.turning_value_ != otherMyClass.turning_value_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("ContinuousHikingCommandMessage {"); + builder.append("enable_continuous_hiking="); + builder.append(this.enable_continuous_hiking_); builder.append(", "); + builder.append("steps_before_safety_stop="); + builder.append(this.steps_before_safety_stop_); builder.append(", "); + builder.append("walk_forwards="); + builder.append(this.walk_forwards_); builder.append(", "); + builder.append("square_up_to_goal="); + builder.append(this.square_up_to_goal_); builder.append(", "); + builder.append("use_astar_footstep_planner="); + builder.append(this.use_astar_footstep_planner_); builder.append(", "); + builder.append("use_monte_carlo_footstep_planner="); + builder.append(this.use_monte_carlo_footstep_planner_); builder.append(", "); + builder.append("use_previous_plan_as_reference="); + builder.append(this.use_previous_plan_as_reference_); builder.append(", "); + builder.append("use_monte_carlo_plan_as_reference="); + builder.append(this.use_monte_carlo_plan_as_reference_); builder.append(", "); + builder.append("use_joystick_controller="); + builder.append(this.use_joystick_controller_); builder.append(", "); + builder.append("forward_value="); + builder.append(this.forward_value_); builder.append(", "); + builder.append("walk_backwards="); + builder.append(this.walk_backwards_); builder.append(", "); + builder.append("lateral_value="); + builder.append(this.lateral_value_); builder.append(", "); + builder.append("turning_value="); + builder.append(this.turning_value_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessagePubSubType.java similarity index 61% rename from ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessagePubSubType.java rename to ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessagePubSubType.java index 3af3fcf3e3d..674b89fe3c0 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousHikingCommandMessagePubSubType.java @@ -2,20 +2,20 @@ /** * -* Topic data type of the struct "ContinuousWalkingCommandMessage" defined in "ContinuousWalkingCommandMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* Topic data type of the struct "ContinuousHikingCommandMessage" defined in "ContinuousHikingCommandMessage_.idl". Use this class to provide the TopicDataType to a Participant. * -* This file was automatically generated from ContinuousWalkingCommandMessage_.idl by us.ihmc.idl.generator.IDLGenerator. -* Do not update this file directly, edit ContinuousWalkingCommandMessage_.idl instead. +* This file was automatically generated from ContinuousHikingCommandMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit ContinuousHikingCommandMessage_.idl instead. * */ -public class ContinuousWalkingCommandMessagePubSubType implements us.ihmc.pubsub.TopicDataType +public class ContinuousHikingCommandMessagePubSubType implements us.ihmc.pubsub.TopicDataType { - public static final java.lang.String name = "behavior_msgs::msg::dds_::ContinuousWalkingCommandMessage_"; + public static final java.lang.String name = "behavior_msgs::msg::dds_::ContinuousHikingCommandMessage_"; @Override public final java.lang.String getDefinitionChecksum() { - return "f758c83c4b6c80fb6df0a3ecfd60a8f3a93372821d16b63c3e75fd063d7b8700"; + return "16a67ca29bda8379d5278ae1f141a59cf18a01a63fe6d9d13557e347e9445765"; } @Override @@ -28,7 +28,7 @@ public final java.lang.String getDefinitionVersion() private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); @Override - public void serialize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + public void serialize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException { serializeCDR.serialize(serializedPayload); write(data, serializeCDR); @@ -36,7 +36,7 @@ public void serialize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data } @Override - public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data) throws java.io.IOException + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.ContinuousHikingCommandMessage data) throws java.io.IOException { deserializeCDR.deserialize(serializedPayload); read(data, deserializeCDR); @@ -54,15 +54,13 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); - - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); @@ -72,37 +70,43 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + return current_alignment - initial_alignment; } - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data) + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data) { return getCdrSerializedSize(data, 0); } - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, int current_alignment) + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, int current_alignment) { int initial_alignment = current_alignment; current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); @@ -114,31 +118,31 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.ContinuousWal current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); - return current_alignment - initial_alignment; - } - public static void write(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.idl.CDR cdr) - { - cdr.write_type_7(data.getEnableContinuousHikingWithKeyboard()); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); - cdr.write_type_7(data.getEnableContinuousHikingWithJoystickController()); - cdr.write_type_7(data.getWalkBackwards()); - cdr.write_type_6(data.getForwardValue()); + return current_alignment - initial_alignment; + } - cdr.write_type_6(data.getLateralValue()); + public static void write(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_7(data.getEnableContinuousHiking()); - cdr.write_type_6(data.getTurningValue()); + cdr.write_type_4(data.getStepsBeforeSafetyStop()); - cdr.write_type_7(data.getUseHybridPlanner()); + cdr.write_type_7(data.getWalkForwards()); + + cdr.write_type_7(data.getSquareUpToGoal()); cdr.write_type_7(data.getUseAstarFootstepPlanner()); @@ -148,23 +152,27 @@ public static void write(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage d cdr.write_type_7(data.getUseMonteCarloPlanAsReference()); + cdr.write_type_7(data.getUseJoystickController()); + + cdr.write_type_6(data.getForwardValue()); + + cdr.write_type_7(data.getWalkBackwards()); + + cdr.write_type_6(data.getLateralValue()); + + cdr.write_type_6(data.getTurningValue()); + } - public static void read(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.idl.CDR cdr) + public static void read(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.idl.CDR cdr) { - data.setEnableContinuousHikingWithKeyboard(cdr.read_type_7()); + data.setEnableContinuousHiking(cdr.read_type_7()); - data.setEnableContinuousHikingWithJoystickController(cdr.read_type_7()); + data.setStepsBeforeSafetyStop(cdr.read_type_4()); - data.setWalkBackwards(cdr.read_type_7()); + data.setWalkForwards(cdr.read_type_7()); - data.setForwardValue(cdr.read_type_6()); - - data.setLateralValue(cdr.read_type_6()); - - data.setTurningValue(cdr.read_type_6()); - - data.setUseHybridPlanner(cdr.read_type_7()); + data.setSquareUpToGoal(cdr.read_type_7()); data.setUseAstarFootstepPlanner(cdr.read_type_7()); @@ -174,50 +182,64 @@ public static void read(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage da data.setUseMonteCarloPlanAsReference(cdr.read_type_7()); + data.setUseJoystickController(cdr.read_type_7()); + + data.setForwardValue(cdr.read_type_6()); + + data.setWalkBackwards(cdr.read_type_7()); + + data.setLateralValue(cdr.read_type_6()); + + data.setTurningValue(cdr.read_type_6()); + } @Override - public final void serialize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.idl.InterchangeSerializer ser) + public final void serialize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.idl.InterchangeSerializer ser) { - ser.write_type_7("enable_continuous_hiking_with_keyboard", data.getEnableContinuousHikingWithKeyboard()); - ser.write_type_7("enable_continuous_hiking_with_joystick_controller", data.getEnableContinuousHikingWithJoystickController()); - ser.write_type_7("walk_backwards", data.getWalkBackwards()); - ser.write_type_6("forward_value", data.getForwardValue()); - ser.write_type_6("lateral_value", data.getLateralValue()); - ser.write_type_6("turning_value", data.getTurningValue()); - ser.write_type_7("use_hybrid_planner", data.getUseHybridPlanner()); + ser.write_type_7("enable_continuous_hiking", data.getEnableContinuousHiking()); + ser.write_type_4("steps_before_safety_stop", data.getStepsBeforeSafetyStop()); + ser.write_type_7("walk_forwards", data.getWalkForwards()); + ser.write_type_7("square_up_to_goal", data.getSquareUpToGoal()); ser.write_type_7("use_astar_footstep_planner", data.getUseAstarFootstepPlanner()); ser.write_type_7("use_monte_carlo_footstep_planner", data.getUseMonteCarloFootstepPlanner()); ser.write_type_7("use_previous_plan_as_reference", data.getUsePreviousPlanAsReference()); ser.write_type_7("use_monte_carlo_plan_as_reference", data.getUseMonteCarloPlanAsReference()); + ser.write_type_7("use_joystick_controller", data.getUseJoystickController()); + ser.write_type_6("forward_value", data.getForwardValue()); + ser.write_type_7("walk_backwards", data.getWalkBackwards()); + ser.write_type_6("lateral_value", data.getLateralValue()); + ser.write_type_6("turning_value", data.getTurningValue()); } @Override - public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data) + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.ContinuousHikingCommandMessage data) { - data.setEnableContinuousHikingWithKeyboard(ser.read_type_7("enable_continuous_hiking_with_keyboard")); - data.setEnableContinuousHikingWithJoystickController(ser.read_type_7("enable_continuous_hiking_with_joystick_controller")); - data.setWalkBackwards(ser.read_type_7("walk_backwards")); - data.setForwardValue(ser.read_type_6("forward_value")); - data.setLateralValue(ser.read_type_6("lateral_value")); - data.setTurningValue(ser.read_type_6("turning_value")); - data.setUseHybridPlanner(ser.read_type_7("use_hybrid_planner")); + data.setEnableContinuousHiking(ser.read_type_7("enable_continuous_hiking")); + data.setStepsBeforeSafetyStop(ser.read_type_4("steps_before_safety_stop")); + data.setWalkForwards(ser.read_type_7("walk_forwards")); + data.setSquareUpToGoal(ser.read_type_7("square_up_to_goal")); data.setUseAstarFootstepPlanner(ser.read_type_7("use_astar_footstep_planner")); data.setUseMonteCarloFootstepPlanner(ser.read_type_7("use_monte_carlo_footstep_planner")); data.setUsePreviousPlanAsReference(ser.read_type_7("use_previous_plan_as_reference")); data.setUseMonteCarloPlanAsReference(ser.read_type_7("use_monte_carlo_plan_as_reference")); + data.setUseJoystickController(ser.read_type_7("use_joystick_controller")); + data.setForwardValue(ser.read_type_6("forward_value")); + data.setWalkBackwards(ser.read_type_7("walk_backwards")); + data.setLateralValue(ser.read_type_6("lateral_value")); + data.setTurningValue(ser.read_type_6("turning_value")); } - public static void staticCopy(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage src, behavior_msgs.msg.dds.ContinuousWalkingCommandMessage dest) + public static void staticCopy(behavior_msgs.msg.dds.ContinuousHikingCommandMessage src, behavior_msgs.msg.dds.ContinuousHikingCommandMessage dest) { dest.set(src); } @Override - public behavior_msgs.msg.dds.ContinuousWalkingCommandMessage createData() + public behavior_msgs.msg.dds.ContinuousHikingCommandMessage createData() { - return new behavior_msgs.msg.dds.ContinuousWalkingCommandMessage(); + return new behavior_msgs.msg.dds.ContinuousHikingCommandMessage(); } @Override public int getTypeSize() @@ -231,24 +253,24 @@ public java.lang.String getName() return name; } - public void serialize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.idl.CDR cdr) + public void serialize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.idl.CDR cdr) { write(data, cdr); } - public void deserialize(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage data, us.ihmc.idl.CDR cdr) + public void deserialize(behavior_msgs.msg.dds.ContinuousHikingCommandMessage data, us.ihmc.idl.CDR cdr) { read(data, cdr); } - public void copy(behavior_msgs.msg.dds.ContinuousWalkingCommandMessage src, behavior_msgs.msg.dds.ContinuousWalkingCommandMessage dest) + public void copy(behavior_msgs.msg.dds.ContinuousHikingCommandMessage src, behavior_msgs.msg.dds.ContinuousHikingCommandMessage dest) { staticCopy(src, dest); } @Override - public ContinuousWalkingCommandMessagePubSubType newInstance() + public ContinuousHikingCommandMessagePubSubType newInstance() { - return new ContinuousWalkingCommandMessagePubSubType(); + return new ContinuousHikingCommandMessagePubSubType(); } } diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessage.java deleted file mode 100644 index 838ba6d55c4..00000000000 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/ContinuousWalkingCommandMessage.java +++ /dev/null @@ -1,367 +0,0 @@ -package behavior_msgs.msg.dds; - -import us.ihmc.communication.packets.Packet; -import us.ihmc.euclid.interfaces.Settable; -import us.ihmc.euclid.interfaces.EpsilonComparable; -import java.util.function.Supplier; -import us.ihmc.pubsub.TopicDataType; - -public class ContinuousWalkingCommandMessage extends Packet implements Settable, EpsilonComparable -{ - /** - * flag to enable/disable continuous hiking state machine with keyboard - */ - public boolean enable_continuous_hiking_with_keyboard_; - /** - * flag to enable/disable continuous hiking state machine with joystick controller - */ - public boolean enable_continuous_hiking_with_joystick_controller_; - /** - * flag to walk backwards with the continuous hiking state machine - */ - public boolean walk_backwards_; - /** - * forward joystick value - */ - public double forward_value_; - /** - * lateral joystick value - */ - public double lateral_value_; - /** - * turning joystick value - */ - public double turning_value_; - /** - * flag to enable/disable hybrid planning - */ - public boolean use_hybrid_planner_; - /** - * flag to enable/disable planning with astar planner - */ - public boolean use_astar_footstep_planner_; - /** - * flag to enable/disable planning with monte-carlo footstep planner - */ - public boolean use_monte_carlo_footstep_planner_; - /** - * flag to enable/disable using previous plan as reference - */ - public boolean use_previous_plan_as_reference_; - /** - * flag to enable/disable using monte-carlo plan as reference - */ - public boolean use_monte_carlo_plan_as_reference_; - - public ContinuousWalkingCommandMessage() - { - } - - public ContinuousWalkingCommandMessage(ContinuousWalkingCommandMessage other) - { - this(); - set(other); - } - - public void set(ContinuousWalkingCommandMessage other) - { - enable_continuous_hiking_with_keyboard_ = other.enable_continuous_hiking_with_keyboard_; - - enable_continuous_hiking_with_joystick_controller_ = other.enable_continuous_hiking_with_joystick_controller_; - - walk_backwards_ = other.walk_backwards_; - - forward_value_ = other.forward_value_; - - lateral_value_ = other.lateral_value_; - - turning_value_ = other.turning_value_; - - use_hybrid_planner_ = other.use_hybrid_planner_; - - use_astar_footstep_planner_ = other.use_astar_footstep_planner_; - - use_monte_carlo_footstep_planner_ = other.use_monte_carlo_footstep_planner_; - - use_previous_plan_as_reference_ = other.use_previous_plan_as_reference_; - - use_monte_carlo_plan_as_reference_ = other.use_monte_carlo_plan_as_reference_; - - } - - /** - * flag to enable/disable continuous hiking state machine with keyboard - */ - public void setEnableContinuousHikingWithKeyboard(boolean enable_continuous_hiking_with_keyboard) - { - enable_continuous_hiking_with_keyboard_ = enable_continuous_hiking_with_keyboard; - } - /** - * flag to enable/disable continuous hiking state machine with keyboard - */ - public boolean getEnableContinuousHikingWithKeyboard() - { - return enable_continuous_hiking_with_keyboard_; - } - - /** - * flag to enable/disable continuous hiking state machine with joystick controller - */ - public void setEnableContinuousHikingWithJoystickController(boolean enable_continuous_hiking_with_joystick_controller) - { - enable_continuous_hiking_with_joystick_controller_ = enable_continuous_hiking_with_joystick_controller; - } - /** - * flag to enable/disable continuous hiking state machine with joystick controller - */ - public boolean getEnableContinuousHikingWithJoystickController() - { - return enable_continuous_hiking_with_joystick_controller_; - } - - /** - * flag to walk backwards with the continuous hiking state machine - */ - public void setWalkBackwards(boolean walk_backwards) - { - walk_backwards_ = walk_backwards; - } - /** - * flag to walk backwards with the continuous hiking state machine - */ - public boolean getWalkBackwards() - { - return walk_backwards_; - } - - /** - * forward joystick value - */ - public void setForwardValue(double forward_value) - { - forward_value_ = forward_value; - } - /** - * forward joystick value - */ - public double getForwardValue() - { - return forward_value_; - } - - /** - * lateral joystick value - */ - public void setLateralValue(double lateral_value) - { - lateral_value_ = lateral_value; - } - /** - * lateral joystick value - */ - public double getLateralValue() - { - return lateral_value_; - } - - /** - * turning joystick value - */ - public void setTurningValue(double turning_value) - { - turning_value_ = turning_value; - } - /** - * turning joystick value - */ - public double getTurningValue() - { - return turning_value_; - } - - /** - * flag to enable/disable hybrid planning - */ - public void setUseHybridPlanner(boolean use_hybrid_planner) - { - use_hybrid_planner_ = use_hybrid_planner; - } - /** - * flag to enable/disable hybrid planning - */ - public boolean getUseHybridPlanner() - { - return use_hybrid_planner_; - } - - /** - * flag to enable/disable planning with astar planner - */ - public void setUseAstarFootstepPlanner(boolean use_astar_footstep_planner) - { - use_astar_footstep_planner_ = use_astar_footstep_planner; - } - /** - * flag to enable/disable planning with astar planner - */ - public boolean getUseAstarFootstepPlanner() - { - return use_astar_footstep_planner_; - } - - /** - * flag to enable/disable planning with monte-carlo footstep planner - */ - public void setUseMonteCarloFootstepPlanner(boolean use_monte_carlo_footstep_planner) - { - use_monte_carlo_footstep_planner_ = use_monte_carlo_footstep_planner; - } - /** - * flag to enable/disable planning with monte-carlo footstep planner - */ - public boolean getUseMonteCarloFootstepPlanner() - { - return use_monte_carlo_footstep_planner_; - } - - /** - * flag to enable/disable using previous plan as reference - */ - public void setUsePreviousPlanAsReference(boolean use_previous_plan_as_reference) - { - use_previous_plan_as_reference_ = use_previous_plan_as_reference; - } - /** - * flag to enable/disable using previous plan as reference - */ - public boolean getUsePreviousPlanAsReference() - { - return use_previous_plan_as_reference_; - } - - /** - * flag to enable/disable using monte-carlo plan as reference - */ - public void setUseMonteCarloPlanAsReference(boolean use_monte_carlo_plan_as_reference) - { - use_monte_carlo_plan_as_reference_ = use_monte_carlo_plan_as_reference; - } - /** - * flag to enable/disable using monte-carlo plan as reference - */ - public boolean getUseMonteCarloPlanAsReference() - { - return use_monte_carlo_plan_as_reference_; - } - - - public static Supplier getPubSubType() - { - return ContinuousWalkingCommandMessagePubSubType::new; - } - - @Override - public Supplier getPubSubTypePacket() - { - return ContinuousWalkingCommandMessagePubSubType::new; - } - - @Override - public boolean epsilonEquals(ContinuousWalkingCommandMessage other, double epsilon) - { - if(other == null) return false; - if(other == this) return true; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_continuous_hiking_with_keyboard_, other.enable_continuous_hiking_with_keyboard_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_continuous_hiking_with_joystick_controller_, other.enable_continuous_hiking_with_joystick_controller_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.walk_backwards_, other.walk_backwards_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.forward_value_, other.forward_value_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.lateral_value_, other.lateral_value_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.turning_value_, other.turning_value_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_hybrid_planner_, other.use_hybrid_planner_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_astar_footstep_planner_, other.use_astar_footstep_planner_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_monte_carlo_footstep_planner_, other.use_monte_carlo_footstep_planner_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_previous_plan_as_reference_, other.use_previous_plan_as_reference_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_monte_carlo_plan_as_reference_, other.use_monte_carlo_plan_as_reference_, epsilon)) return false; - - - return true; - } - - @Override - public boolean equals(Object other) - { - if(other == null) return false; - if(other == this) return true; - if(!(other instanceof ContinuousWalkingCommandMessage)) return false; - - ContinuousWalkingCommandMessage otherMyClass = (ContinuousWalkingCommandMessage) other; - - if(this.enable_continuous_hiking_with_keyboard_ != otherMyClass.enable_continuous_hiking_with_keyboard_) return false; - - if(this.enable_continuous_hiking_with_joystick_controller_ != otherMyClass.enable_continuous_hiking_with_joystick_controller_) return false; - - if(this.walk_backwards_ != otherMyClass.walk_backwards_) return false; - - if(this.forward_value_ != otherMyClass.forward_value_) return false; - - if(this.lateral_value_ != otherMyClass.lateral_value_) return false; - - if(this.turning_value_ != otherMyClass.turning_value_) return false; - - if(this.use_hybrid_planner_ != otherMyClass.use_hybrid_planner_) return false; - - if(this.use_astar_footstep_planner_ != otherMyClass.use_astar_footstep_planner_) return false; - - if(this.use_monte_carlo_footstep_planner_ != otherMyClass.use_monte_carlo_footstep_planner_) return false; - - if(this.use_previous_plan_as_reference_ != otherMyClass.use_previous_plan_as_reference_) return false; - - if(this.use_monte_carlo_plan_as_reference_ != otherMyClass.use_monte_carlo_plan_as_reference_) return false; - - - return true; - } - - @Override - public java.lang.String toString() - { - StringBuilder builder = new StringBuilder(); - - builder.append("ContinuousWalkingCommandMessage {"); - builder.append("enable_continuous_hiking_with_keyboard="); - builder.append(this.enable_continuous_hiking_with_keyboard_); builder.append(", "); - builder.append("enable_continuous_hiking_with_joystick_controller="); - builder.append(this.enable_continuous_hiking_with_joystick_controller_); builder.append(", "); - builder.append("walk_backwards="); - builder.append(this.walk_backwards_); builder.append(", "); - builder.append("forward_value="); - builder.append(this.forward_value_); builder.append(", "); - builder.append("lateral_value="); - builder.append(this.lateral_value_); builder.append(", "); - builder.append("turning_value="); - builder.append(this.turning_value_); builder.append(", "); - builder.append("use_hybrid_planner="); - builder.append(this.use_hybrid_planner_); builder.append(", "); - builder.append("use_astar_footstep_planner="); - builder.append(this.use_astar_footstep_planner_); builder.append(", "); - builder.append("use_monte_carlo_footstep_planner="); - builder.append(this.use_monte_carlo_footstep_planner_); builder.append(", "); - builder.append("use_previous_plan_as_reference="); - builder.append(this.use_previous_plan_as_reference_); builder.append(", "); - builder.append("use_monte_carlo_plan_as_reference="); - builder.append(this.use_monte_carlo_plan_as_reference_); - builder.append("}"); - return builder.toString(); - } -} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousHikingCommandMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousHikingCommandMessage.msg new file mode 100644 index 00000000000..dfd5748b673 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousHikingCommandMessage.msg @@ -0,0 +1,39 @@ +# Flag to start Continuous Hiking +bool enable_continuous_hiking + +# Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever +uint32 steps_before_safety_stop + +# Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick +bool walk_forwards + +# Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. +# Setting this to true tells the robot to step on the final goal positions. +bool square_up_to_goal + +# Flag to enable/disable planning with A* planner. This is the default planner to use +bool use_astar_footstep_planner + +# Flag to enable/disable planning with monte-carlo footstep planner +bool use_monte_carlo_footstep_planner + +# Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time +bool use_previous_plan_as_reference + +# Flag to enable/disable using monte-carlo plan as reference +bool use_monte_carlo_plan_as_reference + +# Walk with the Joystick Controller +bool use_joystick_controller + +# Forward joystick value +float64 forward_value + +# Backward walking from Joystick Controller +bool walk_backwards + +# Lateral joystick value +float64 lateral_value + +# Turning joystick value +float64 turning_value \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg deleted file mode 100644 index 06f33bbd265..00000000000 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg +++ /dev/null @@ -1,32 +0,0 @@ -# flag to enable/disable continuous hiking state machine with keyboard -bool enable_continuous_hiking_with_keyboard - -# flag to enable/disable continuous hiking state machine with joystick controller -bool enable_continuous_hiking_with_joystick_controller - -# flag to walk backwards with the continuous hiking state machine -bool walk_backwards - -# forward joystick value -float64 forward_value - -# lateral joystick value -float64 lateral_value - -# turning joystick value -float64 turning_value - -# flag to enable/disable hybrid planning -bool use_hybrid_planner - -# flag to enable/disable planning with astar planner -bool use_astar_footstep_planner - -# flag to enable/disable planning with monte-carlo footstep planner -bool use_monte_carlo_footstep_planner - -# flag to enable/disable using previous plan as reference -bool use_previous_plan_as_reference - -# flag to enable/disable using monte-carlo plan as reference -bool use_monte_carlo_plan_as_reference \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousHikingCommandMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousHikingCommandMessage.msg new file mode 100644 index 00000000000..88334f079c4 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousHikingCommandMessage.msg @@ -0,0 +1,42 @@ + +# Flag to start Continuous Hiking +bool enable_continuous_hiking + +# Flag that allows the user to set a number of steps that will be taken, and Continuous Hiking will stop after that. This way the robot doesn't walk forever +uint32 steps_before_safety_stop + +# Flag to walk straight forward with Continuous Hiking. This is generally a keyboard input, and is different then trying to walk with the Joystick +bool walk_forwards + +# Flag to square up to the goal when finishing walking. We may want the robot to end at an exact location. +# Setting this to true tells the robot to step on the final goal positions. +bool square_up_to_goal + +# Flag to enable/disable planning with A* planner. This is the default planner to use +bool use_astar_footstep_planner + +# Flag to enable/disable planning with monte-carlo footstep planner +bool use_monte_carlo_footstep_planner + +# Flag to enable/disable using previous plan as reference. Generally this should be set to true to reduce planning time +bool use_previous_plan_as_reference + +# Flag to enable/disable using monte-carlo plan as reference +bool use_monte_carlo_plan_as_reference + +# Walk with the Joystick Controller +bool use_joystick_controller + +# Forward joystick value +float64 forward_value + +# Backward walking from Joystick Controller +bool walk_backwards + +# Lateral joystick value +float64 lateral_value + +# Turning joystick value +float64 turning_value + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg deleted file mode 100644 index da383bd705e..00000000000 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/ContinuousWalkingCommandMessage.msg +++ /dev/null @@ -1,35 +0,0 @@ - -# flag to enable/disable continuous hiking state machine with keyboard -bool enable_continuous_hiking_with_keyboard - -# flag to enable/disable continuous hiking state machine with joystick controller -bool enable_continuous_hiking_with_joystick_controller - -# flag to walk backwards with the continuous hiking state machine -bool walk_backwards - -# forward joystick value -float64 forward_value - -# lateral joystick value -float64 lateral_value - -# turning joystick value -float64 turning_value - -# flag to enable/disable hybrid planning -bool use_hybrid_planner - -# flag to enable/disable planning with astar planner -bool use_astar_footstep_planner - -# flag to enable/disable planning with monte-carlo footstep planner -bool use_monte_carlo_footstep_planner - -# flag to enable/disable using previous plan as reference -bool use_previous_plan_as_reference - -# flag to enable/disable using monte-carlo plan as reference -bool use_monte_carlo_plan_as_reference - - From 2785ec9bf0ea8d46acf880f732c8d86df35a03d8 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sun, 24 Nov 2024 12:26:40 -0600 Subject: [PATCH 2/4] Updated parameters and API for Continuous Hiking --- .../communication/ContinuousWalkingAPI.java | 18 ++++-------------- .../ContinuousHikingParameters.java | 1 - .../ContinuousHikingParametersBasics.java | 5 ----- .../ContinuousHikingParametersReadOnly.java | 5 ----- .../ContinuousHikingParameters.json | 1 - 5 files changed, 4 insertions(+), 26 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java index 3a405d22a9d..141f3833608 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java @@ -1,12 +1,11 @@ package us.ihmc.footstepPlanning.communication; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; -import controller_msgs.msg.dds.RigidBodyTransformMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; +import std_msgs.msg.dds.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; -import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.ros2.ROS2Topic; public class ContinuousWalkingAPI @@ -15,19 +14,10 @@ public class ContinuousWalkingAPI public static final ROS2Topic IHMC_ROOT = new ROS2Topic<>().withPrefix(IHMC_TOPIC_PREFIX); private static final String ACTIVE_MODULE_NAME = "active_perception"; - public static final SideDependentList> FOOTSTEP_PLANNING_START = new SideDependentList<>( - IHMC_ROOT.withModule("continuous_walking").withType(RigidBodyTransformMessage.class).withSuffix("footstep_planning_start_left"), - IHMC_ROOT.withModule("continuous_walking").withType(RigidBodyTransformMessage.class).withSuffix("footstep_planning_start_right") - ); - - public static final SideDependentList> FOOTSTEP_PLANNING_GOAL = new SideDependentList<>( - IHMC_ROOT.withModule("continuous_walking").withType(RigidBodyTransformMessage.class).withSuffix("footstep_planning_goal_left"), - IHMC_ROOT.withModule("continuous_walking").withType(RigidBodyTransformMessage.class).withSuffix("footstep_planning_goal_right") - ); - - public static final ROS2Topic CONTINUOUS_WALKING_COMMAND = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousWalkingCommandMessage.class).withSuffix("command"); + public static final ROS2Topic CONTINUOUS_WALKING_COMMAND = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousHikingCommandMessage.class).withSuffix("command"); public static final ROS2Topic CONTINUOUS_WALKING_STATUS = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousWalkingStatusMessage.class).withSuffix("status"); + public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); public static final ROS2Topic PLANNED_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(FootstepDataListMessage.class).withSuffix("planned_footsteps"); public static final ROS2Topic START_AND_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(PoseListMessage.class).withSuffix("start_and_goal"); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java index 8dfc012ad5f..c057eee1fe6 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java @@ -18,7 +18,6 @@ public class ContinuousHikingParameters extends StoredPropertySet implements Con { public static final StoredPropertyKeyList keys = new StoredPropertyKeyList(); - public static final BooleanStoredPropertyKey enableContinuousHiking = keys.addBooleanKey("Enable continuous hiking"); public static final BooleanStoredPropertyKey stepPublisherEnabled = keys.addBooleanKey("Step publisher enabled"); public static final BooleanStoredPropertyKey overrideEntireQueueEachStep = keys.addBooleanKey("Override entire queue each step"); public static final IntegerStoredPropertyKey numberOfStepsToSend = keys.addIntegerKey("Number of steps to send"); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java index 1fba1aef983..4860c31305d 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java @@ -8,11 +8,6 @@ */ public interface ContinuousHikingParametersBasics extends ContinuousHikingParametersReadOnly, StoredPropertySetBasics { - default void setEnableContinuousHiking(boolean enableContinuousHiking) - { - set(ContinuousHikingParameters.enableContinuousHiking, enableContinuousHiking); - } - default void setStepPublisherEnabled(boolean stepPublisherEnabled) { set(ContinuousHikingParameters.stepPublisherEnabled, stepPublisherEnabled); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java index ed3127d2de4..ecf1c13533c 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java @@ -10,11 +10,6 @@ */ public interface ContinuousHikingParametersReadOnly extends StoredPropertySetReadOnly { - default boolean getEnableContinuousHiking() - { - return get(enableContinuousHiking); - } - default boolean getStepPublisherEnabled() { return get(stepPublisherEnabled); diff --git a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json index 86bddf8ebe5..86aa0543ae0 100644 --- a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json +++ b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json @@ -1,6 +1,5 @@ { "title" : "Continuous Hiking parameters", - "Enable continuous hiking" : false, "Step publisher enabled" : false, "Override entire queue each step" : true, "Number of steps to send" : 3, From 4cdd3e3249393a0fde8d6175c5b90218a9b204e5 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sun, 24 Nov 2024 12:28:14 -0600 Subject: [PATCH 3/4] Update the Continuous Hiking state machine to use the updated message and parameters, fixed bugs with walking to a user specified goal --- .../perception/RDXContinuousHikingPanel.java | 152 +++++++++----- .../ActivePlanarMappingRemoteTask.java | 37 ++-- .../ReadyToPlanState.java | 186 ++++++++---------- ...rtContinuousHikingTransitionCondition.java | 13 +- ...opContinuousHikingTransitionCondition.java | 12 +- .../WaitingToLandState.java | 18 ++ .../activeMapping/ContinuousPlanner.java | 73 ++++--- .../ContinuousPlannerSchedulingTask.java | 30 +-- .../TerrainPlanningDebugger.java | 30 +-- 9 files changed, 277 insertions(+), 274 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 459001ac6d8..df886ecb9f8 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -1,6 +1,6 @@ package us.ihmc.rdx.perception; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import com.badlogic.gdx.controllers.Controller; import com.badlogic.gdx.controllers.Controllers; import com.badlogic.gdx.graphics.g3d.Renderable; @@ -40,6 +40,7 @@ import us.ihmc.perception.comms.PerceptionComms; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.rdx.imgui.ImGuiSliderDouble; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySetGroup; @@ -63,22 +64,23 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private static final int numberOfKnotPoints = 12; private static final int maxIterationsOptimization = 100; private final ROS2Node ros2Node; + private final ROS2Helper ros2Helper; private final DRCRobotModel robotModel; private final ROS2SyncedRobotModel syncedRobotModel; - private final ROS2PublisherBasics commandPublisher; - private final ContinuousWalkingCommandMessage commandMessage = new ContinuousWalkingCommandMessage(); + private final ROS2PublisherBasics commandPublisher; + private final ContinuousHikingCommandMessage commandMessage = new ContinuousHikingCommandMessage(); private final RDXStancePoseSelectionPanel stancePoseSelectionPanel; private final PositionOptimizedTrajectoryGenerator positionTrajectoryGenerator = new PositionOptimizedTrajectoryGenerator(numberOfKnotPoints, maxIterationsOptimization); private final RDXTerrainPlanningDebugger terrainPlanningDebugger; - private final ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); private final SwingTrajectoryParameters swingTrajectoryParameters; private final RDXStoredPropertySetTuner continuousHikingParametersPanel = new RDXStoredPropertySetTuner("Continuous Hiking Parameters (CH)"); private final ImGuiRemoteROS2StoredPropertySetGroup hostStoredPropertySets; - private final ImBoolean localRenderMode = new ImBoolean(false); - private final ImBoolean useMonteCarloReference = new ImBoolean(false); - private final ImBoolean useHybridPlanner = new ImBoolean(false); + private final ImGuiSliderDouble stepsBeforeSafetyStop = new ImGuiSliderDouble("Steps Before Safety Stop", "%.2f"); + private final ImBoolean enableContinuousHiking = new ImBoolean(false); + private final ImBoolean squareUpToGoal = new ImBoolean(false); private final ImBoolean useAStarFootstepPlanner = new ImBoolean(true); + private final ImBoolean useMonteCarloReference = new ImBoolean(false); private final ImBoolean useMonteCarloFootstepPlanner = new ImBoolean(false); private SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private FootstepPlan latestFootstepPlan; @@ -92,6 +94,7 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper ros2Helper, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { super("Continuous Hiking"); + this.ros2Helper = ros2Helper; setRenderMethod(this::renderImGuiWidgets); this.ros2Node = ros2Node; @@ -131,6 +134,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper message -> terrainPlanningDebugger.reset()); hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, hostStoredPropertySets, @@ -220,7 +224,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData) /** * This method handles updating the stored property sets used in Continuous Hiking. * These are all the parameters that are getting synced back and forth between the remote process and the local process. - * There are three situations that can occur when tyring to use Continuous Hiking. + * There are three situations that can occur when trying to use Continuous Hiking. *
    *
  • Case 1: The situation where we are simulating the process running on a remote machine but in reality its running locally. * This is where we only want to update the property sets running on that process. Represented by {@link #clientStoredPropertySets}. @@ -229,7 +233,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData) * Here we want to publish, and subscribe in one place as everything is being run on the same machine. * So we update {@link #clientStoredPropertySets} and {@link #hostStoredPropertySets}
    • *
  • Case 3: Then the situation where we only want to publish the property sets to be sent to the remote process. - * This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these chagnes
    • + * This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these changes * *
*/ @@ -258,13 +262,41 @@ public void renderImGuiWidgets() ImGui.separator(); continuousHikingParametersPanel.renderImGuiWidgets(); - ImGui.checkbox("Local Render Mode", localRenderMode); + ImGui.separator(); + ImGui.text("Options for Continuous Hiking Message"); + ImGui.indent(); + ImGui.checkbox("Enable Continuous Hiking", enableContinuousHiking); + ImGui.checkbox("Square Up To Goal", squareUpToGoal); + if (ImGui.button("Clear Planned footsteps")) + { + clearPlannedFootsteps(); + } + ImGui.sameLine(); + stepsBeforeSafetyStop.render(0.0, 50.0); ImGui.checkbox("Use A* Footstep Planner", useAStarFootstepPlanner); ImGui.checkbox("Use Monte-Carlo Footstep Planner", useMonteCarloFootstepPlanner); ImGui.checkbox("Use Monte-Carlo Reference", useMonteCarloReference); + ImGui.unindent(); ImGui.separator(); terrainPlanningDebugger.renderImGuiWidgets(); - publishInputCommandMessage(); + + // Check to see if a controller is plugged into the computer + Controller joystickController = Controllers.getCurrent(); + // Here we check against null rather then .isConnected() because if the controller is unplugged, that method won't work + boolean controllerConnected = joystickController != null; + + if (ImGui.getIO().getKeyAlt()) + { + publishStopContinuousHiking(); + } + else if (controllerConnected) + { + publishJoystickStatus(joystickController); + } + else if (ImGui.getIO().getKeyCtrl() && ImGui.getIO().getKeyShift()) + { + publishContinuousHikingCommand(); + } } public void processImGui3DViewInput(ImGui3DViewInput input) @@ -279,6 +311,11 @@ public void getRenderables(Array renderables, Pool pool) terrainPlanningDebugger.getRenderables(renderables, pool); } + public void clearPlannedFootsteps() + { + ros2Helper.publish(ContinuousWalkingAPI.CLEAR_GOAL_FOOTSTEPS); + } + /** * We have received the start and goal pose from the process, lets unpack this message and visualize the start and goal on the UI. */ @@ -327,49 +364,70 @@ public void onMonteCarloPlanReceived(FootstepDataListMessage message) terrainPlanningDebugger.generateMonteCarloPlanGraphic(message); } - private void publishInputCommandMessage() + private void publishStopContinuousHiking() { - // Check to see if a controller is plugged into the computer - Controller joystickController = Controllers.getCurrent(); - // Here we check against null rather then .isConnected() because if the controller is unplugged that method won't work - boolean controllerConnected = joystickController != null; + commandMessage.setEnableContinuousHiking(false); + commandMessage.setStepsBeforeSafetyStop(0); + commandMessage.setWalkForwards(false); + commandMessage.setSquareUpToGoal(false); + commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); + commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); + commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); + commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); + + commandMessage.setUseJoystickController(false); + commandMessage.setForwardValue(0.0); + commandMessage.setWalkBackwards(false); + commandMessage.setLateralValue(0.0); + commandMessage.setTurningValue(0.0); + + commandPublisher.publish(commandMessage); + } - // Setup a bunch of variables to be published in the message - boolean walkWithKeyboard = ImGui.getIO().getKeyCtrl(); - boolean walkWithController = false; + private void publishJoystickStatus(Controller joystickController) + { + + // Setup variables to be published in the message boolean walkBackwards = false; - double lateralJoystickValue = 0.0; double forwardJoystickValue = 0.0; + double lateralJoystickValue = 0.0; double turningJoystickValue = 0.0; - if (controllerConnected) - { - walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB); - walkWithController = joystickController.getButton(joystickController.getMapping().buttonA); - walkWithController |= walkBackwards; - forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY); - lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX); - turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX); - } + walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB); + forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY); + lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX); + turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX); - // Only allow Continuous Walking if the CTRL key is held and the checkbox is checked - // We publish this all the time to prevent any of the values from staying true all the time - if (continuousHikingParameters.getEnableContinuousHiking()) - { - commandMessage.setEnableContinuousHikingWithKeyboard(walkWithKeyboard); - commandMessage.setEnableContinuousHikingWithJoystickController(walkWithController); - commandMessage.setWalkBackwards(walkBackwards); - commandMessage.setForwardValue(forwardJoystickValue); - commandMessage.setLateralValue(lateralJoystickValue); - commandMessage.setTurningValue(turningJoystickValue); - commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); - commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); - commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); - commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); - commandMessage.setUseHybridPlanner(useHybridPlanner.get()); - - commandPublisher.publish(commandMessage); - } + commandMessage.setUseJoystickController(true); + commandMessage.setForwardValue(forwardJoystickValue); + commandMessage.setWalkBackwards(walkBackwards); + commandMessage.setLateralValue(lateralJoystickValue); + commandMessage.setTurningValue(turningJoystickValue); + + commandPublisher.publish(commandMessage); + } + + /** + * Here we want to publish and walk forward, that is the point of pressing the keys on the keyboard + */ + private void publishContinuousHikingCommand() + { + commandMessage.setEnableContinuousHiking(enableContinuousHiking.get()); + commandMessage.setStepsBeforeSafetyStop((int) stepsBeforeSafetyStop.getDoubleValue()); + commandMessage.setWalkForwards(true); + commandMessage.setSquareUpToGoal(squareUpToGoal.get()); + commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); + commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); + commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); + commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); + + commandMessage.setUseJoystickController(false); + commandMessage.setForwardValue(0.0); + commandMessage.setWalkBackwards(false); + commandMessage.setLateralValue(0.0); + commandMessage.setTurningValue(0.0); + + commandPublisher.publish(commandMessage); } public void destroy() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java index 653c671b67e..75320d90337 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java @@ -5,7 +5,6 @@ import perception_msgs.msg.dds.FramePlanarRegionsListMessage; import perception_msgs.msg.dds.ImageMessage; import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask.PlanningMode; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ros2.ROS2PublisherMap; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; @@ -27,13 +26,11 @@ public class ActivePlanarMappingRemoteTask extends LocalizationAndMappingTask private final AtomicReference walkingStatusMessage = new AtomicReference<>(new WalkingStatusMessage()); private final ROS2PublisherMap publisherMap; private final ROS2Topic controllerFootstepDataTopic; - - private ContinuousPlannerForPlanarRegions continuousPlanner; private final ContinuousHikingParameters continuousPlanningParameters; private final SwingPlannerParametersBasics swingFootPlannerParameters; + private ContinuousPlannerForPlanarRegions continuousPlanner; private TerrainPlanningDebugger terrainPlanningDebugger; - public ActivePlanarMappingRemoteTask(String simpleRobotName, DRCRobotModel robotModel, ContinuousHikingParameters continuousPlanningParameters, @@ -47,7 +44,7 @@ public ActivePlanarMappingRemoteTask(String simpleRobotName, super(simpleRobotName, terrainRegionsTopic, structuralRegionsTopic, ros2Node, referenceFrames, referenceFramesUpdater, smoothing); this.walkingStatusMessage.get().setWalkingStatus(WalkingStatus.COMPLETED.toByte()); - this.terrainPlanningDebugger = new TerrainPlanningDebugger(ros2Node, null, PlanningMode.FAST_HIKING); + this.terrainPlanningDebugger = new TerrainPlanningDebugger(ros2Node, null); this.continuousPlanningParameters = continuousPlanningParameters; this.swingFootPlannerParameters = robotModel.getSwingPlannerParameters(); this.controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, robotModel.getSimpleRobotName()); @@ -68,9 +65,6 @@ public void onOusterDepthReceived(ImageMessage imageMessage) { // Convert Ouster depth image to pointcloud - - - } private void walkingStatusReceived(WalkingStatusMessage walkingStatusMessage) @@ -84,7 +78,7 @@ private void walkingStatusReceived(WalkingStatusMessage walkingStatusMessage) */ private void generalUpdate() { -// activeMappingModule.setActive(configurationParameters.getSLAMEnabled()); + // activeMappingModule.setActive(configurationParameters.getSLAMEnabled()); //activeMappingModule.getPlanarRegionMap().printStatistics(true); } @@ -93,26 +87,23 @@ private void generalUpdate() */ private void updateActiveMappingPlan() { - if (continuousPlanningParameters.getEnableContinuousHiking()) + if (walkingStatusMessage.get() != null) { - if (walkingStatusMessage.get() != null) + if (walkingStatusMessage.get().getWalkingStatus() == WalkingStatusMessage.COMPLETED && !continuousPlanner.isPlanAvailable()) { - if (walkingStatusMessage.get().getWalkingStatus() == WalkingStatusMessage.COMPLETED && !continuousPlanner.isPlanAvailable()) - { - continuousPlanner.planBodyPathWithPlanarRegionMap(planarRegionMap); - } + continuousPlanner.planBodyPathWithPlanarRegionMap(planarRegionMap); } + } - if (continuousPlanner.isPlanAvailable()) - { - // Publishing Plan Result - FootstepDataListMessage footstepDataList = continuousPlanner.getFootstepDataListMessage(); - publisherMap.publish(controllerFootstepDataTopic, footstepDataList); + if (continuousPlanner.isPlanAvailable()) + { + // Publishing Plan Result + FootstepDataListMessage footstepDataList = continuousPlanner.getFootstepDataListMessage(); + publisherMap.publish(controllerFootstepDataTopic, footstepDataList); - continuousPlanner.setPlanAvailable(false); - } -// configurationParameters.setActiveMapping(false); + continuousPlanner.setPlanAvailable(false); } + // configurationParameters.setActiveMapping(false); } public ContinuousPlannerForPlanarRegions getContinuousPlanner() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java index d187cfca470..edb9e469014 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java @@ -1,13 +1,12 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; import org.apache.commons.lang3.time.StopWatch; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; -import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask.PlanningMode; import us.ihmc.behaviors.activeMapping.ContinuousPlannerTools; import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.communication.packets.MessageTools; @@ -37,7 +36,7 @@ public class ReadyToPlanState implements State private static final float NOMINAL_STANCE_WIDTH = 0.22f; private final HumanoidReferenceFrames referenceFrames; - private final AtomicReference commandMessage; + private final AtomicReference commandMessage; private final ContinuousPlanner continuousPlanner; private final ControllerFootstepQueueMonitor controllerFootstepQueueMonitor; private final ContinuousHikingParameters continuousHikingParameters; @@ -48,8 +47,7 @@ public class ReadyToPlanState implements State private final Point3D robotLocation = new Point3D(); private final StopWatch stopWatch = new StopWatch(); double timeInSwingToStopPlanningAndWaitTillNextAttempt = 0; - - private PlanningMode planningMode; + private boolean wasWalkingTowardsGoal = false; /** * This state exists to plan footsteps based on the conditions of the {@link ContinuousHikingParameters}. This state publishes visuals the UI but doesn't @@ -60,14 +58,13 @@ public class ReadyToPlanState implements State */ public ReadyToPlanState(ROS2Helper ros2Helper, HumanoidReferenceFrames referenceFrames, - AtomicReference commandMessage, + AtomicReference commandMessage, ContinuousPlanner continuousPlanner, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, ContinuousHikingParameters continuousHikingParameters, TerrainMapData terrainMap, TerrainPlanningDebugger debugger, - ContinuousHikingLogger continuousHikingLogger, - PlanningMode planningMode) + ContinuousHikingLogger continuousHikingLogger) { this.referenceFrames = referenceFrames; this.commandMessage = commandMessage; @@ -77,9 +74,9 @@ public ReadyToPlanState(ROS2Helper ros2Helper, this.terrainMap = terrainMap; this.debugger = debugger; this.continuousHikingLogger = continuousHikingLogger; - this.planningMode = planningMode; ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.PLACED_GOAL_FOOTSTEPS, this::addWayPointPoseToList); + ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.CLEAR_GOAL_FOOTSTEPS, this::clearWayPointList); } @Override @@ -108,11 +105,10 @@ public void doAction(double timeInState) debugger.publishStartAndGoalForVisualization(continuousPlanner.getStartStancePose(), goalPoses); // Plan to the goal and log the plan - continuousPlanner.planToGoal(commandMessage.get(), goalPoses); + continuousPlanner.planToGoal(goalPoses); continuousPlanner.logFootStePlan(); - if (commandMessage.get().getUseHybridPlanner() || commandMessage.get().getUseMonteCarloFootstepPlanner() || commandMessage.get() - .getUseMonteCarloPlanAsReference()) + if (commandMessage.get().getUseMonteCarloFootstepPlanner() || commandMessage.get().getUseMonteCarloPlanAsReference()) { debugger.publishMonteCarloPlan(continuousPlanner.getMonteCarloFootstepDataListMessage()); debugger.publishMonteCarloNodesForVisualization(continuousPlanner.getMonteCarloFootstepPlanner().getRoot(), terrainMap); @@ -146,101 +142,83 @@ public boolean isDone(double timeInState) public SideDependentList getGoalPosesBasedOnPlanningMode() { String message = commandMessage.get().toString(); - continuousHikingLogger.appendString("Command Message that was published: \n" + message); - SideDependentList goalPoses = new SideDependentList<>(); + continuousHikingLogger.appendString("Continuous Hiking Command Being Used: \n" + message); + + SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(), new FramePose3D()); - switch (this.planningMode) + if (wasWalkingTowardsGoal && walkToGoalWayPointPoses.isEmpty()) { - case FAST_HIKING -> - { - if (commandMessage.get().getEnableContinuousHikingWithKeyboard()) - { - goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - else if (commandMessage.get().getEnableContinuousHikingWithJoystickController()) - { - if (commandMessage.get().getWalkBackwards()) - { - goalPoses = ContinuousPlannerTools.setStraightBackwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseBackwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - // Here we assume the joystick isn't being turned at all, so we give a direction of straight forward - else if (Math.abs(commandMessage.get().getLateralValue()) < 0.1) - { - goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - else - { - goalPoses = ContinuousPlannerTools.setGoalPoseBasedOnLateralJoystickValue(referenceFrames.getPelvisZUpFrame(), - referenceFrames.getMidFeetZUpFrame(), - commandMessage.get().getLateralValue(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - NOMINAL_STANCE_WIDTH); + commandMessage.get().setEnableContinuousHiking(false); + // debugger.resetVisualizationForUIPublisher(); + wasWalkingTowardsGoal = false; + } + else if (!walkToGoalWayPointPoses.isEmpty()) + { + wasWalkingTowardsGoal = true; + // Allow for more planning time with this one, just plan for one-step length + continuousHikingParameters.setPlanningWithoutReferenceTimeout(1.0); - // We update this pose because when we start walking straight forward again, it's from the point where we are currently - // And not the point from which we were at before we started turning - FramePose3D stanceMidPose = new FramePose3D(); - stanceMidPose.interpolate(continuousPlanner.getStartStancePose().get(RobotSide.LEFT), - continuousPlanner.getStartStancePose().get(RobotSide.RIGHT), - 0.5); - continuousPlanner.setWalkingStartMidPose(stanceMidPose); - } - } + // Set the goalPoses here so that we return a good value regardless of what happens next + goalPoses = walkToGoalWayPointPoses.get(0); - return goalPoses; - } + // Update the current robot location + Vector3DBasics robotLocationVector = referenceFrames.getMidFeetZUpFrame().getTransformToWorldFrame().getTranslation(); + robotLocation.set(robotLocationVector); + double distanceToGoalPose = ContinuousPlannerTools.getDistanceFromRobotToGoalPoseOnXYPlane(robotLocation, goalPoses); - // This allows for walking to a goal that isn't straight forward; its assumed that if there is no goal we will just resume walking straight forward - case WALK_TO_GOAL -> + if (distanceToGoalPose < continuousHikingParameters.getNextWaypointDistanceMargin()) { - // Allow for more planning time with this one, just plan for one-step length - continuousHikingParameters.setPlanningWithoutReferenceTimeout(1.0); - - // Set the goalPoses here so that we return a good value regardless of what happens next - goalPoses = walkToGoalWayPointPoses.get(0); - - // Update the current robot location - Vector3DBasics robotLocationVector = referenceFrames.getMidFeetZUpFrame().getTransformToWorldFrame().getTranslation(); - robotLocation.set(robotLocationVector); - - double distanceToGoalPose = ContinuousPlannerTools.getDistanceFromRobotToGoalPoseOnXYPlane(robotLocation, goalPoses); - - if (distanceToGoalPose < continuousHikingParameters.getNextWaypointDistanceMargin()) - { - walkToGoalWayPointPoses.remove(0); - - if (!walkToGoalWayPointPoses.isEmpty()) - { - goalPoses = walkToGoalWayPointPoses.get(0); - } - else - { - // We do this here because as soon as continuous hiking gets set to false, we exit this state - planningMode = PlanningMode.FAST_HIKING; - debugger.setPlanningMode(planningMode); - debugger.resetVisualizationForUIPublisher(); - continuousHikingParameters.setEnableContinuousHiking(false); - } - } - - return goalPoses; + walkToGoalWayPointPoses.remove(0); + } + } + else if (commandMessage.get().getUseJoystickController()) + { + if (commandMessage.get().getWalkBackwards()) + { + goalPoses = ContinuousPlannerTools.setStraightBackwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseBackwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } + // Here we assume the joystick isn't being turned at all, so we give a direction of straight forward + else if (Math.abs(commandMessage.get().getLateralValue()) < 0.1) + { + goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } + else + { + goalPoses = ContinuousPlannerTools.setGoalPoseBasedOnLateralJoystickValue(referenceFrames.getPelvisZUpFrame(), + referenceFrames.getMidFeetZUpFrame(), + commandMessage.get().getLateralValue(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + NOMINAL_STANCE_WIDTH); + + // We update this pose because when we start walking straight forward again, it's from the point where we are currently at + // And not the point from which we were at before we started turning + FramePose3D stanceMidPose = new FramePose3D(); + stanceMidPose.interpolate(continuousPlanner.getStartStancePose().get(RobotSide.LEFT), + continuousPlanner.getStartStancePose().get(RobotSide.RIGHT), + 0.5); + continuousPlanner.setWalkingStartMidPose(stanceMidPose); } } + else + { + goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } return goalPoses; } @@ -254,13 +232,21 @@ public void addWayPointPoseToList(PoseListMessage poseListMessage) leftFootPose.set(poses.get(0)); rightFootPose.set(poses.get(1)); - planningMode = PlanningMode.WALK_TO_GOAL; + wasWalkingTowardsGoal = true; SideDependentList latestWayPoint = new SideDependentList<>(); latestWayPoint.put(RobotSide.LEFT, leftFootPose); latestWayPoint.put(RobotSide.RIGHT, rightFootPose); LogTools.info("Added waypoint for WALK_TO_GOAL"); walkToGoalWayPointPoses.add(latestWayPoint); + continuousPlanner.setImminentStanceToPlanFrom(); debugger.publishStartAndGoalForVisualization(continuousPlanner.getStartStancePose(), latestWayPoint); } + + public void clearWayPointList() + { + walkToGoalWayPointPoses.clear(); + wasWalkingTowardsGoal = false; + commandMessage.get().setEnableContinuousHiking(false); + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java index 3bab3bc87cc..6030c64d400 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java @@ -1,6 +1,6 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.robotics.stateMachine.core.StateTransitionCondition; @@ -8,25 +8,20 @@ public class StartContinuousHikingTransitionCondition implements StateTransitionCondition { - private final AtomicReference commandMessage; - private final ContinuousHikingParameters continuousHikingParameters; + private final AtomicReference commandMessage; /** * This transition is used in the {@link us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask} to determine whether the Continuous Hiking state * machine should be started. */ - public StartContinuousHikingTransitionCondition(AtomicReference commandMessage, - ContinuousHikingParameters continuousHikingParameters) + public StartContinuousHikingTransitionCondition(AtomicReference commandMessage) { this.commandMessage = commandMessage; - this.continuousHikingParameters = continuousHikingParameters; } @Override public boolean testCondition(double timeInCurrentState) { - // Both conditions have to be true in order for this to work. The makes things a bit safer to use and can prevent accidentally starting things and having the robot walk - return continuousHikingParameters.getEnableContinuousHiking() && (commandMessage.get().getEnableContinuousHikingWithKeyboard() || commandMessage.get() - .getEnableContinuousHikingWithJoystickController()); + return commandMessage.get().getEnableContinuousHiking(); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java index bc4c012c8ee..95d2a41673b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java @@ -1,6 +1,6 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.robotics.stateMachine.core.StateTransitionCondition; @@ -8,24 +8,20 @@ public class StopContinuousHikingTransitionCondition implements StateTransitionCondition { - private final AtomicReference commandMessage; - private final ContinuousHikingParameters continuousHikingParameters; + private final AtomicReference commandMessage; /** * This transition is used in the {@link us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask} to determine whether the Continuous Hiking state * machine should be stopped. We want to be able to stop the state machine from whatever state we are in. */ - public StopContinuousHikingTransitionCondition(AtomicReference commandMessage, - ContinuousHikingParameters continuousHikingParameters) + public StopContinuousHikingTransitionCondition(AtomicReference commandMessage) { this.commandMessage = commandMessage; - this.continuousHikingParameters = continuousHikingParameters; } @Override public boolean testCondition(double timeInCurrentState) { - return !continuousHikingParameters.getEnableContinuousHiking() || !(commandMessage.get().getEnableContinuousHikingWithKeyboard() || commandMessage.get() - .getEnableContinuousHikingWithJoystickController()); + return !commandMessage.get().getEnableContinuousHiking(); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java index c9b8e4ab40b..d94189214ce 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java @@ -1,5 +1,6 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepStatusMessage; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; @@ -12,9 +13,12 @@ import us.ihmc.robotics.stateMachine.core.State; import us.ihmc.ros2.ROS2Topic; +import java.util.concurrent.atomic.AtomicReference; + public class WaitingToLandState implements State { private final ROS2Helper ros2Helper; + private final AtomicReference commandMessage; private final ContinuousHikingParameters continuousHikingParameters; private final ROS2Topic controllerFootstepDataTopic; @@ -22,6 +26,7 @@ public class WaitingToLandState implements State private final ControllerFootstepQueueMonitor controllerQueueMonitor; private FootstepStatusMessage previousFootstepStatusMessage = null; private final ContinuousHikingLogger continuousHikingLogger; + private boolean isDone = false; /** * This state exists to send a plan to the controller (that is if we have a plan to send). Then we will exist this state when the next step is started. Once @@ -29,12 +34,14 @@ public class WaitingToLandState implements State */ public WaitingToLandState(ROS2Helper ros2Helper, String simpleRobotName, + AtomicReference commandMessage, ContinuousPlanner continuousPlanner, ControllerFootstepQueueMonitor controllerQueueMonitor, ContinuousHikingParameters continuousHikingParameters, ContinuousHikingLogger continuousHikingLogger) { this.ros2Helper = ros2Helper; + this.commandMessage = commandMessage; this.continuousHikingParameters = continuousHikingParameters; this.continuousPlanner = continuousPlanner; this.controllerQueueMonitor = controllerQueueMonitor; @@ -47,6 +54,8 @@ public WaitingToLandState(ROS2Helper ros2Helper, @Override public void onEntry() { + isDone = false; + if (continuousHikingParameters.getStepPublisherEnabled()) { if (continuousPlanner.isPlanAvailable()) @@ -76,6 +85,10 @@ public void onEntry() @Override public void doAction(double timeInState) { + if (controllerQueueMonitor.getControllerFootstepQueue() != null && controllerQueueMonitor.getControllerFootstepQueue().isEmpty() && !continuousPlanner.isInitialized() && !continuousPlanner.isPlanAvailable()) + { + isDone = true; + } } @Override @@ -86,6 +99,11 @@ public void onExit(double timeInState) @Override public boolean isDone(double timeInState) { + if (isDone) + { + return true; + } + //TODO this is a bit messy, cleanup please if (controllerQueueMonitor.getFootstepStatusMessage() != null) { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java index 77a8674da5a..e0686591602 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java @@ -1,6 +1,6 @@ package us.ihmc.behaviors.activeMapping; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepStatusMessage; import controller_msgs.msg.dds.QueuedFootstepStatusMessage; @@ -46,21 +46,19 @@ public class ContinuousPlanner private final FootstepPlanningModule footstepPlanner; private final MonteCarloFootstepPlanner monteCarloFootstepPlanner; private final AtomicReference monteCarloFootstepPlan = new AtomicReference<>(null); - private FootstepPlan monteCarloReferencePlan; - private FootstepPlan previousFootstepPlan; - private FootstepPlan latestFootstepPlan; - private FootstepPlanningResult footstepPlanningResult; - private final ContinuousHikingParameters continuousHikingParameters; private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; private final SwingPlannerParametersBasics swingPlannerParameters; - private final SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private final FramePose3D walkingStartMidPose = new FramePose3D(); private final FramePose3D imminentFootstepPose = new FramePose3D(); + private FootstepPlan monteCarloReferencePlan; + private FootstepPlan previousFootstepPlan; + private FootstepPlan latestFootstepPlan; + private FootstepPlanningResult footstepPlanningResult; private RobotSide imminentFootstepSide = RobotSide.LEFT; - private ContinuousWalkingCommandMessage command; + private final AtomicReference commandMessage; private AtomicReference latestFootstepStatusMessage = new AtomicReference<>(new FootstepStatusMessage()); private List controllerQueue = new ArrayList<>(); @@ -73,6 +71,7 @@ public class ContinuousPlanner public ContinuousPlanner(DRCRobotModel robotModel, HumanoidReferenceFrames referenceFrames, + AtomicReference command, ContinuousHikingParameters continuousHikingParameters, MonteCarloFootstepPlannerParameters monteCarloPlannerParameters, DefaultFootstepPlannerParametersBasics footstepPlannerParameters, @@ -81,6 +80,7 @@ public ContinuousPlanner(DRCRobotModel robotModel, ContinuousHikingLogger continuousHikingLogger) { this.referenceFrames = referenceFrames; + this.commandMessage = command; this.continuousHikingParameters = continuousHikingParameters; this.footstepPlannerParameters = footstepPlannerParameters; this.swingPlannerParameters = swingPlannerParameters; @@ -114,40 +114,20 @@ public void initialize() initialized = true; } - /** - * This pose represents the starting position where the robot started walking from. - * We save this value because we are planning over and over again, and we - * want to keep some notion of where we started from so we can walk in a straight line from there. - * - * @param startingPose is the pose from where we want to walk in a straight line from - */ - public void setWalkingStartMidPose(FramePose3D startingPose) - { - walkingStartMidPose.getPosition().set(startingPose.getPosition()); - walkingStartMidPose.getOrientation().setToYawOrientation(startingPose.getRotation().getYaw()); - } - - public void planToGoal(ContinuousWalkingCommandMessage command, SideDependentList goalPoses) + public void planToGoal(SideDependentList goalPoses) { - this.command = command; - - if (command.getUseAstarFootstepPlanner()) - { - latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, command.getUsePreviousPlanAsReference(), false, goalPoses); - } - else if (command.getUseMonteCarloFootstepPlanner()) - { - latestFootstepPlan = generateMonteCarloFootstepPlan(goalPoses); - } - else if (command.getUseHybridPlanner()) + if (commandMessage.get().getUseAstarFootstepPlanner()) { - generateMonteCarloFootstepPlan(goalPoses); latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, - command.getUsePreviousPlanAsReference(), - command.getUseMonteCarloPlanAsReference(), + commandMessage.get().getUsePreviousPlanAsReference(), + false, goalPoses); } + else if (commandMessage.get().getUseMonteCarloFootstepPlanner()) + { + latestFootstepPlan = generateMonteCarloFootstepPlan(goalPoses); + } else { latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, true, false, goalPoses); @@ -180,7 +160,7 @@ public FootstepPlan generateAStarFootstepPlan(HeightMapData heightMapData, request.setAbortIfGoalStepSnappingFails(true); // When walking backwards, we want to keep the body facing in the same direction, otherwise the robot will turn as it tries to step backwards - if (command.getWalkBackwards()) + if (commandMessage.get().getWalkBackwards()) { FramePose3D bodyMidGoalPose = new FramePose3D(); bodyMidGoalPose.interpolate(goalPoses.get(RobotSide.LEFT), goalPoses.get(RobotSide.RIGHT), 0.5); @@ -285,7 +265,7 @@ else if (usePreviousPlanAsReference && previousFootstepPlan != null) public void logFootStePlan() { - // In case logging footstep plans becomes a problem, we have this feature where we can not log plans if we want too + // In case logging footstep plans becomes a problem, we have this feature where we cannot log plans if we want too if (continuousHikingParameters.getLogFootstepPlans()) { logger.logSession(); @@ -413,7 +393,7 @@ public void setImminentStanceToPlanFrom() public void getImminentStanceFromLatestStatus(AtomicReference footstepStatusMessage, List controllerQueue) { - // Sometimes no message exists, by default ignore the message if its null and use what ever the imminent side was last time + // Sometimes no message exists, by default ignore the message if its null and use whatever the imminent side was last time RobotSide imminentFootSide = imminentFootstepSide; RobotSide sideFromMessage = RobotSide.fromByte(footstepStatusMessage.get().getRobotSide()); if (sideFromMessage != null) @@ -473,7 +453,7 @@ public void transitionCallback() continuousHikingLogger.appendString("[TRANSITION]: Resetting Previous Plan Reference"); this.previousFootstepPlan = new FootstepPlan(latestFootstepPlan); - if (command.getUseMonteCarloFootstepPlanner()) + if (commandMessage.get().getUseMonteCarloFootstepPlanner()) { monteCarloFootstepPlanner.transitionToOptimal(); } @@ -539,6 +519,19 @@ public FramePose3D getWalkingStartMidPose() return walkingStartMidPose; } + /** + * This pose represents the starting position where the robot started walking from. + * We save this value because we are planning over and over again, and we + * want to keep some notion of where we started from, so we can walk in a straight line from there. + * + * @param startingPose is the pose from where we want to walk in a straight line from + */ + public void setWalkingStartMidPose(FramePose3D startingPose) + { + walkingStartMidPose.getPosition().set(startingPose.getPosition()); + walkingStartMidPose.getOrientation().setToYawOrientation(startingPose.getRotation().getYaw()); + } + public void setLatestHeightMapData(HeightMapData heightMapData) { this.latestHeightMapData = heightMapData; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java index e4bf5aa1013..47c15086164 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java @@ -1,6 +1,6 @@ package us.ihmc.behaviors.activeMapping; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine.*; import us.ihmc.communication.ros2.ROS2Helper; @@ -33,19 +33,9 @@ public class ContinuousPlannerSchedulingTask * This is the delay between each tick of the state machine. Set based on perception update rate. */ private final static long CONTINUOUS_PLANNING_DELAY_MS = 16; - - public enum PlanningMode - { - FAST_HIKING, WALK_TO_GOAL - } - - // The default mode for when things start up - private PlanningMode planningMode = PlanningMode.FAST_HIKING; - protected final ScheduledExecutorService executorService = ExecutorServiceTools.newScheduledThreadPool(1, getClass(), ExecutorServiceTools.ExceptionHandling.CATCH_AND_REPORT); - private final ContinuousPlanner continuousPlanner; public StateMachine stateMachine; private TerrainMapData terrainMap; @@ -61,13 +51,14 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, String simpleRobotName = robotModel.getSimpleRobotName(); ROS2Helper ros2Helper = new ROS2Helper(ros2Node); - AtomicReference commandMessage = new AtomicReference<>(new ContinuousWalkingCommandMessage()); + AtomicReference commandMessage = new AtomicReference<>(new ContinuousHikingCommandMessage()); ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.CONTINUOUS_WALKING_COMMAND, commandMessage::set); - TerrainPlanningDebugger debugger = new TerrainPlanningDebugger(ros2Node, monteCarloFootstepPlannerParameters, planningMode); + TerrainPlanningDebugger debugger = new TerrainPlanningDebugger(ros2Node, monteCarloFootstepPlannerParameters); ContinuousHikingLogger continuousHikingLogger = new ContinuousHikingLogger(); continuousPlanner = new ContinuousPlanner(robotModel, referenceFrames, + commandMessage, continuousHikingParameters, monteCarloFootstepPlannerParameters, footstepPlannerParameters, @@ -96,10 +87,10 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, continuousHikingParameters, terrainMap, debugger, - continuousHikingLogger, - planningMode); + continuousHikingLogger); State waitingtoLandState = new WaitingToLandState(ros2Helper, simpleRobotName, + commandMessage, continuousPlanner, controllerFootstepQueueMonitor, continuousHikingParameters, @@ -111,10 +102,8 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, stateMachineFactory.addState(ContinuousHikingState.WAITING_TO_LAND, waitingtoLandState); // Create different conditions - StartContinuousHikingTransitionCondition startContinuousHikingTransitionCondition = new StartContinuousHikingTransitionCondition(commandMessage, - continuousHikingParameters); - StopContinuousHikingTransitionCondition stopContinuousHikingTransitionCondition = new StopContinuousHikingTransitionCondition(commandMessage, - continuousHikingParameters); + StartContinuousHikingTransitionCondition startContinuousHikingTransitionCondition = new StartContinuousHikingTransitionCondition(commandMessage); + StopContinuousHikingTransitionCondition stopContinuousHikingTransitionCondition = new StopContinuousHikingTransitionCondition(commandMessage); PlanAgainTransitionCondition planAgainTransitionCondition = new PlanAgainTransitionCondition(continuousPlanner, continuousHikingParameters); //NOTE: The transitions for the state machine are checked in the order they are added. @@ -139,11 +128,10 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, stateMachine = stateMachineFactory.build(ContinuousHikingState.DO_NOTHING); stateMachineFactory.addStateChangedListener((from, to) -> { - String message ="STATE CHANGED: (" + from + " -> " + to + ")"; + String message = "STATE CHANGED: (" + from + " -> " + to + ")"; LogTools.warn(message); continuousHikingLogger.appendString(message); }); - stateMachineFactory.addStateChangedListener((from, to) -> planningMode = debugger.getPlanningMode()); stateMachineFactory.addStateChangedListener((from, to) -> continuousHikingLogger.logToFile(true, false)); executorService.scheduleWithFixedDelay(this::tickStateMachine, 1500, CONTINUOUS_PLANNING_DELAY_MS, TimeUnit.MILLISECONDS); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java index dbe88ff2701..fdf6d1e37b1 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java @@ -7,7 +7,6 @@ import org.bytedeco.opencv.global.opencv_imgproc; import org.bytedeco.opencv.opencv_core.Mat; import org.bytedeco.opencv.opencv_core.Size; -import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask.PlanningMode; import us.ihmc.communication.packets.MessageTools; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.referenceFrame.FramePose3D; @@ -64,14 +63,12 @@ public class TerrainPlanningDebugger private ROS2PublisherBasics monteCarloNodesPublisherForUI; private MonteCarloFootstepPlannerRequest request; private MonteCarloFootstepPlannerParameters parameters; - private PlanningMode planningMode; private Mat contactHeatMapImage; - public TerrainPlanningDebugger(ROS2Node ros2Node, MonteCarloFootstepPlannerParameters parameters, PlanningMode planningMode) + public TerrainPlanningDebugger(ROS2Node ros2Node, MonteCarloFootstepPlannerParameters parameters) { this.parameters = parameters; - this.planningMode = planningMode; if (ros2Node != null) { plannedFootstesPublisherForUI = ros2Node.createPublisher(ContinuousWalkingAPI.PLANNED_FOOTSTEPS); @@ -263,18 +260,9 @@ public void resetVisualizationForUIPublisher() PoseListMessage poseListMessage = new PoseListMessage(); FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); - if (planningMode == PlanningMode.FAST_HIKING) - { - startAndGoalPublisherForUI.publish(poseListMessage); - monteCarloNodesPublisherForUI.publish(poseListMessage); - plannedFootstesPublisherForUI.publish(footstepDataListMessage); - } - - if (planningMode == PlanningMode.WALK_TO_GOAL) - { - monteCarloNodesPublisherForUI.publish(poseListMessage); - plannedFootstesPublisherForUI.publish(footstepDataListMessage); - } + startAndGoalPublisherForUI.publish(poseListMessage); + monteCarloNodesPublisherForUI.publish(poseListMessage); + plannedFootstesPublisherForUI.publish(footstepDataListMessage); } public void publishMonteCarloNodesForVisualization(MonteCarloTreeNode root, TerrainMapData terrainMap) @@ -347,16 +335,6 @@ public Mat getDisplayImage() return stacked; } - public PlanningMode getPlanningMode() - { - return planningMode; - } - - public void setPlanningMode(PlanningMode planningMode) - { - this.planningMode = planningMode; - } - public void setEnabled(boolean enabled) { this.enabled = enabled; From 0ad4ea31ccc2ee53e5cd7a9f534262ba8999bd0b Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Fri, 6 Dec 2024 00:37:38 -0600 Subject: [PATCH 4/4] Wrong import for the Empty message was fixed --- .../footstepPlanning/communication/ContinuousWalkingAPI.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java index cbeb871e24c..1e7d0f4dd02 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java @@ -4,7 +4,7 @@ import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; -import std_msgs.Empty; +import std_msgs.msg.dds.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.ros2.ROS2Topic; @@ -16,7 +16,7 @@ public class ContinuousWalkingAPI // Commands supported for the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_WALKING_COMMAND = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousHikingCommandMessage.class).withSuffix("command"); - public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(Empty.class).withSuffix("clear_goal_footsteps"); + public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); // Statuses supported from the Continuous Hiking Process