Skip to content

Carter marble dropper #216

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

Open
CarterKreis wants to merge 4 commits into
base: Simulation-Localization-Carlos
Choose a base branch
from
Open

Carter marble dropper #216

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4b5fab6
Marble spawns on 'm' press and despawns after time passes
CarterKreis Jul 31, 2025
0bab647
Yeah it fully runs :). Press 'm' to spawn a marble underneath the sub…
CarterKreis Jul 31, 2025
208dceb
Removed some 'cout' statements
CarterKreis Aug 3, 2025
6a972f6
plz
CarterKreis Oct 9, 2025
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 ext/au
Open in desktop
Submodule au added at 50fb7c
3 changes: 2 additions & 1 deletion src/subjugator/command/subjugator_keyboard_control/src/SubjugatorKeyboardControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@ SubjugatorKeyboardControl::SubjugatorKeyboardControl()
, running_(true)
{
publisher_ = this->create_publisher<geometry_msgs::msg::Wrench>("cmd_wrench", PUBLISH_RATE);
// Add publisher for keypress events
keypress_publisher_ = this->create_publisher<std_msgs::msg::String>("keyboard/keypress", 10);
this->declare_parameter("linear_speed", 100.0);
this->declare_parameter("angular_speed", 100.0);
base_linear_ = this->get_parameter("linear_speed").as_double();
Expand Down Expand Up @@ -203,7 +205,6 @@ void SubjugatorKeyboardControl::keyboardLoop()
std_msgs::msg::String keypress_msg;
keypress_msg.data = "m";
keypress_publisher_->publish(keypress_msg);
std::cout << "[SubjugatorKeyboardControl] Marble spawn request sent." << std::endl;
break;
}
case 'q':
Expand Down
8 changes: 1 addition & 7 deletions src/subjugator/simulation/subjugator_gazebo/include/MarbleDropper.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,7 @@

namespace marble_dropper
{
class MarbleDropper : public gz::sim::System,
public gz::sim::ISystemConfigure,
public gz::sim::ISystemPreUpdate,
public gz::sim::ISystemPostUpdate
class MarbleDropper : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate
{
public:
MarbleDropper();
Expand All @@ -46,9 +43,6 @@ class MarbleDropper : public gz::sim::System,
void Configure(gz::sim::Entity const &entity, std::shared_ptr<sdf::Element const> const &sdf,
gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) override;

// System PreUpdate - Called every simulation step before physics //
void PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &ecm) override;

// System PostUpdate - Called every simulation step to update pinger/marble_dropper distances //
void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override;

Expand Down
15 changes: 5 additions & 10 deletions src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,23 +43,16 @@ void MarbleDropper::Configure(gz::sim::Entity const &entity, std::shared_ptr<sdf
return false;
});
}
std::cout << "[MarbleDropper] World Name: " << this->worldName << std::endl;
}

void MarbleDropper::KeypressCallback(std_msgs::msg::String::SharedPtr const msg)
{
std::cout << "[MarbleDropper] Keypress received: " << msg->data << std::endl;
if (msg->data == "m" && !m_pressed)
{
m_pressed = true;
}
}

void MarbleDropper::PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &ecm)
{
// Unnecessary if not adding velocity to marble(s)
}

void MarbleDropper::SpawnMarble(std::string const &worldName, std::string const &sdfPath)
{
// Load SDF file as string
Expand Down Expand Up @@ -102,7 +95,7 @@ void MarbleDropper::SpawnMarble(std::string const &worldName, std::string const

// Set the pose to X, Y, Z, and roll, pitch, yaw
// -0.5 offset in Z to not hit sub9
gz::msgs::Set(factoryMsg.mutable_pose(), gz::math::Pose3d(sub9_pose.X(), sub9_pose.Y(), sub9_pose.Z() - 0.5,
gz::msgs::Set(factoryMsg.mutable_pose(), gz::math::Pose3d(sub9_pose.X(), sub9_pose.Y() + 0.5, sub9_pose.Z() - 0.25,
sub9_pose.Roll(), sub9_pose.Pitch(), sub9_pose.Yaw()));

// Send the request to create model in .world
Expand Down Expand Up @@ -155,6 +148,9 @@ void MarbleDropper::PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityC
std::string removeService = "/world/" + worldName + "/remove";
node.Request(removeService, removeMsg, timeout, reply, result);

std::cout << "[MarbleDropper] Removed marble: " << spawnedMarbIter->first << " after " << timeElapsed
<< " seconds." << std::endl;

// Clean up tracking
marbleModelNames.erase(spawnedMarbIter->first);
spawnedMarbIter = marbleSpawnTimes.erase(spawnedMarbIter);
Expand All @@ -178,5 +174,4 @@ void MarbleDropper::PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityC
} // namespace marble_dropper

// Register plugin for Gazebo
GZ_ADD_PLUGIN(marble_dropper::MarbleDropper, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPostUpdate,
gz::sim::ISystemPreUpdate)
GZ_ADD_PLUGIN(marble_dropper::MarbleDropper, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPostUpdate)
Loading