Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Replaced the ContinuousWalkingAPI.java with the ContinuousHikingAPI.java and got things working on hardware #567

Merged
merged 1 commit into from
Dec 27, 2024
Merged

Replaced the ContinuousWalkingAPI.java with the ContinuousHikingAPI.java and got things working on hardware #567

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ...ommunication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySetGroup.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package us.ihmc.communication.property;

import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI;
import us.ihmc.log.LogTools;
import us.ihmc.tools.property.StoredPropertySetBasics;

import java.util.ArrayList;
Expand Down
43 changes: 0 additions & 43 deletions ...p-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java
View file
Open in desktop

This file was deleted.

10 changes: 5 additions & 5 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemotePerceptionUI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters;
import us.ihmc.communication.ros2.ROS2Helper;
import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters;
import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI;
import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI;
import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics;
import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics;
import us.ihmc.perception.comms.PerceptionComms;
Expand Down Expand Up @@ -59,9 +59,9 @@ public RDXRemotePerceptionUI(ROS2Helper ros2Helper)
remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper);

remotePropertySets.registerRemotePropertySet(perceptionConfigurationParameters, PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS);
remotePropertySets.registerRemotePropertySet(continuousHikingParameters, ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS);
remotePropertySets.registerRemotePropertySet(continuousHikingParameters, ContinuousHikingAPI.CONTINUOUS_HIKING_PARAMETERS);
remotePropertySets.registerRemotePropertySet(heightMapParameters, PerceptionComms.HEIGHT_MAP_PARAMETERS);
remotePropertySets.registerRemotePropertySet(monteCarloFootstepPlannerParameters, ContinuousWalkingAPI.MONTE_CARLO_PLANNER_PARAMETERS);
remotePropertySets.registerRemotePropertySet(monteCarloFootstepPlannerParameters, ContinuousHikingAPI.MONTE_CARLO_PLANNER_PARAMETERS);

registerRapidRegionsParameters();
}
Expand Down Expand Up @@ -127,13 +127,13 @@ public DefaultFootstepPlannerParametersBasics getFootstepPlannerParameters()
public void setFootstepPlannerParameters(DefaultFootstepPlannerParametersBasics parameters)
{
this.footstepPlannerParameters = parameters;
remotePropertySets.registerRemotePropertySet(footstepPlannerParameters, ContinuousWalkingAPI.FOOTSTEP_PLANNING_PARAMETERS);
remotePropertySets.registerRemotePropertySet(footstepPlannerParameters, ContinuousHikingAPI.FOOTSTEP_PLANNING_PARAMETERS);
}

public void setSwingPlannerParameters(SwingPlannerParametersBasics parameters)
{
this.swingPlannerParameters = parameters;
remotePropertySets.registerRemotePropertySet(swingPlannerParameters, ContinuousWalkingAPI.SWING_PLANNING_PARAMETERS);
remotePropertySets.registerRemotePropertySet(swingPlannerParameters, ContinuousHikingAPI.SWING_PLANNING_PARAMETERS);
}

public MonteCarloFootstepPlannerParameters getMonteCarloFootstepPlannerParameters()
Expand Down
6 changes: 3 additions & 3 deletions ...gh-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
import us.ihmc.footstepPlanning.FootstepDataMessageConverter;
import us.ihmc.footstepPlanning.FootstepPlan;
import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters;
import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI;
import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI;
import us.ihmc.footstepPlanning.monteCarloPlanning.MonteCarloPlannerTools;
import us.ihmc.footstepPlanning.tools.PlannerTools;
import us.ihmc.log.LogTools;
Expand Down Expand Up @@ -72,8 +72,8 @@ public RDXTerrainPlanningDebugger(ROS2Helper ros2Helper,
{

this.monteCarloFootstepPlannerParameters = monteCarloFootstepPlannerParameters;
ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.MONTE_CARLO_TREE_NODES, this::onMonteCarloTreeNodesReceived);
ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived);
ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES, this::onMonteCarloTreeNodesReceived);
ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived);

goalFootstepGraphics = new SideDependentList<>(new RDXFootstepGraphic(contactPoints, RobotSide.LEFT),
new RDXFootstepGraphic(contactPoints, RobotSide.RIGHT));
Expand Down
69 changes: 39 additions & 30 deletions ...ping/PerceptionBasedContinuousHiking.java → ...ctiveMapping/ContinuousHikingProcess.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,16 @@

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
import us.ihmc.commons.thread.RepeatingTaskThread;
import us.ihmc.communication.PerceptionAPI;
import us.ihmc.communication.property.ROS2StoredPropertySetGroup;
import us.ihmc.communication.ros2.ROS2Helper;
import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters;
import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI;
import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI;
import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics;
import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics;
import us.ihmc.log.LogTools;
import us.ihmc.perception.StandAloneRealsenseProcess;
import us.ihmc.perception.TerrainPerceptionProcessWithDriver;
import us.ihmc.perception.realsense.RealsenseConfiguration;
import us.ihmc.ros2.ROS2Node;
Expand All @@ -18,50 +21,56 @@
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;

public class PerceptionBasedContinuousHiking
public class ContinuousHikingProcess
{
private final ROS2StoredPropertySetGroup ros2PropertySetGroup;
private final TerrainPerceptionProcessWithDriver perceptionTask;
// private final TerrainPerceptionProcessWithDriver perceptionTask;
private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask;

protected final ScheduledExecutorService executorService = ExecutorServiceTools.newScheduledThreadPool(1,
getClass(),
ExecutorServiceTools.ExceptionHandling.CATCH_AND_REPORT);
private final StandAloneRealsenseProcess standAloneRealsenseProcess;

public PerceptionBasedContinuousHiking(DRCRobotModel robotModel)
public ContinuousHikingProcess(DRCRobotModel robotModel)
{
ROS2Node ros2Node = new ROS2NodeBuilder().build("nadia_terrain_perception_node");
ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
ROS2SyncedRobotModel syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node);
syncedRobot.initializeToDefaultRobotInitialSetup(0.0, 0.0, 0.0, 0.0);
ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Helper);

RepeatingTaskThread robotUpdateThread = new RepeatingTaskThread("SyncedRobotUpdate", syncedRobot::update).setFrequencyLimit(30.0);
robotUpdateThread.startRepeating();

// Add Continuous Hiking Parameters to be between the UI and this process
ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters();
ros2PropertySetGroup.registerStoredPropertySet(ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS, continuousHikingParameters);
ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.CONTINUOUS_HIKING_PARAMETERS, continuousHikingParameters);

// Add Monte Carlo Footstep Planner Parameters to be between the UI and this process
MonteCarloFootstepPlannerParameters monteCarloPlannerParameters = new MonteCarloFootstepPlannerParameters();
ros2PropertySetGroup.registerStoredPropertySet(ContinuousWalkingAPI.MONTE_CARLO_PLANNER_PARAMETERS, monteCarloPlannerParameters);
ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.MONTE_CARLO_PLANNER_PARAMETERS, monteCarloPlannerParameters);

// Add A* Footstep Planner Parameters to be between the UI and this process
DefaultFootstepPlannerParametersBasics footstepPlannerParameters = robotModel.getFootstepPlannerParameters("ForContinuousWalking");
ros2PropertySetGroup.registerStoredPropertySet(ContinuousWalkingAPI.FOOTSTEP_PLANNING_PARAMETERS, footstepPlannerParameters);
ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.FOOTSTEP_PLANNING_PARAMETERS, footstepPlannerParameters);

// Add Swing Planner Parameters to be synced between the UI and this process
SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters();
ros2PropertySetGroup.registerStoredPropertySet(ContinuousWalkingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters);

perceptionTask = new TerrainPerceptionProcessWithDriver(robotModel.getSimpleRobotName(),
robotModel.getCollisionBoxProvider(),
robotModel.createFullRobotModel(),
RealsenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ,
ros2PropertySetGroup,
ros2Helper,
PerceptionAPI.D455_DEPTH_IMAGE,
PerceptionAPI.D455_COLOR_IMAGE,
syncedRobot.getReferenceFrames(),
syncedRobot::update);
ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters);

standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, syncedRobot);

// perceptionTask = new TerrainPerceptionProcessWithDriver(robotModel.getSimpleRobotName(),
// robotModel.getCollisionBoxProvider(),
// robotModel.createFullRobotModel(),
// RealsenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ,
// ros2PropertySetGroup,
// ros2Helper,
// PerceptionAPI.D455_DEPTH_IMAGE,
// PerceptionAPI.D455_COLOR_IMAGE,
// syncedRobot.getReferenceFrames(),
// syncedRobot::update);

continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel,
ros2Node,
Expand All @@ -71,33 +80,33 @@ public PerceptionBasedContinuousHiking(DRCRobotModel robotModel)
footstepPlannerParameters,
swingPlannerParameters);

perceptionTask.run();
// perceptionTask.run();

Runtime.getRuntime().addShutdownHook(new Thread(this::destroy, "Shutdown"));

// Add initial delay to get things going in the right order
executorService.scheduleAtFixedRate(this::update, 500, 100, TimeUnit.MILLISECONDS);
executorService.scheduleAtFixedRate(this::update, 8500, 100, TimeUnit.MILLISECONDS);
}

public void update()
{
ros2PropertySetGroup.update();

if (perceptionTask.getHumanoidPerceptionModule().getRapidRegionsExtractor() == null)
if (standAloneRealsenseProcess.getHeightMapManager() == null)
{
return;
}

if (perceptionTask.getHumanoidPerceptionModule().getLatestHeightMapData() != null)
if (standAloneRealsenseProcess.getLatestHeightMapData() != null && standAloneRealsenseProcess.getLatestTerrainMapData() != null)
{
perceptionTask.getHumanoidPerceptionModule().setIsHeightMapDataBeingProcessed(true);
continuousPlannerSchedulingTask.setLatestHeightMapData(perceptionTask.getHumanoidPerceptionModule().getLatestHeightMapData());
continuousPlannerSchedulingTask.setTerrainMapData(perceptionTask.getHumanoidPerceptionModule().getRapidHeightMapExtractor().getTerrainMapData());
perceptionTask.getHumanoidPerceptionModule().setIsHeightMapDataBeingProcessed(false);
continuousPlannerSchedulingTask.setLatestHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData());
continuousPlannerSchedulingTask.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData());
}
}

public void destroy()
{
if (continuousPlannerSchedulingTask != null)
continuousPlannerSchedulingTask.destroy();
continuousPlannerSchedulingTask.destroy();
standAloneRealsenseProcess.destroy();
}
}
7 changes: 6 additions & 1 deletion ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,14 +86,19 @@ public HeightMapData getLatestHeightMapData()
}
}

public TerrainMapData getTerrainMapData()
public TerrainMapData getLatestTerrainMapData()
{
synchronized (heightMapLock)
{
return heightMapManager.getTerrainMapData();
}
}

public RapidHeightMapManager getHeightMapManager()
{
return heightMapManager;
}

@Override
public void kill()
{
Expand Down
38 changes: 29 additions & 9 deletions ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,13 @@
import us.ihmc.communication.packets.Packet;
import us.ihmc.communication.ros2.ROS2DemandGraphNode;
import us.ihmc.communication.ros2.ROS2Helper;
import us.ihmc.perception.heightMap.TerrainMapData;
import us.ihmc.perception.opencl.OpenCLManager;
import us.ihmc.perception.realsense.RealsenseConfiguration;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.ros2.ROS2Node;
import us.ihmc.ros2.ROS2Topic;
import us.ihmc.sensorProcessing.heightMap.HeightMapData;
import us.ihmc.sensors.RealsenseImageSensor;

import java.util.Map;
Expand All @@ -33,6 +36,8 @@ public class StandAloneRealsenseProcess
private ImageSensorPublishThread d455PublishThread;

private final ROS2DemandGraphNode heightMapDemandNode;
private final OpenCLManager openCLManager = new OpenCLManager();
private RapidHeightMapUpdateThread heightMapUpdateThread;

public StandAloneRealsenseProcess(ROS2Node ros2Node, ROS2Helper ros2Helper, ROS2SyncedRobotModel syncedRobot)
{
Expand All @@ -54,24 +59,39 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, ROS2Helper ros2Helper, ROS2

d455PublishThread = new ImageSensorPublishThread(ros2Node, d455Sensor, D455_IMAGE_TOPIC_MAP);
loopOnDemand(d455PublishThread, realsensePublishDemandNode);
}

initializeHeightMap();
initializeHeightMap();
}
}

private void initializeHeightMap()
{
boolean runWithCUDA = false;
RapidHeightMapUpdateThread heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper,
syncedRobot,
syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.LEFT),
syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT),
d455Sensor,
RealsenseImageSensor.DEPTH_IMAGE_KEY,
runWithCUDA);
heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper,
syncedRobot,
syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.LEFT),
syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT),
d455Sensor,
RealsenseImageSensor.DEPTH_IMAGE_KEY,
runWithCUDA);
loopOnDemand(heightMapUpdateThread, heightMapDemandNode);
}

public RapidHeightMapManager getHeightMapManager()
{
return heightMapUpdateThread.getHeightMapManager();
}

public HeightMapData getLatestHeightMapData()
{
return heightMapUpdateThread.getLatestHeightMapData();
}

public TerrainMapData getLatestTerrainMapData()
{
return heightMapUpdateThread.getLatestTerrainMapData();
}

public void destroy()
{
realsenseDemandNode.destroy();
Expand Down
Loading
Loading