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

Adds function to change the frame IDs of published markers. #41

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Adds function to change the frame IDs of published markers. #41

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
2 changes: 2 additions & 0 deletions include/or_rviz/InteractiveMarkerViewer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,8 @@ class InteractiveMarkerViewer : public OpenRAVE::ViewerBase {

bool AddMenuEntryCommand(std::ostream &out, std::istream &in);
bool GetMenuSelectionCommand(std::ostream &out, std::istream &in);
bool SetFrameIdCommand(std::ostream &out, std::istream &in);
bool GetFrameIdCommand(std::ostream &out, std::istream &in);

void GraphHandleRemovedCallback(util::InteractiveMarkerGraphHandle *handle);
void BodyCallback(OpenRAVE::KinBodyPtr kinbody, int flag);
Expand Down
38 changes: 35 additions & 3 deletions src/InteractiveMarkerViewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,14 @@ InteractiveMarkerViewer::InteractiveMarkerViewer(
boost::bind(&InteractiveMarkerViewer::GetMenuSelectionCommand, this, _1, _2),
"Get the name of the last menu selection."
);
RegisterCommand("GetFrameId",
boost::bind(&InteractiveMarkerViewer::GetFrameIdCommand, this, _1, _2),
"Get the frame in which interactive markers are published."
);
RegisterCommand("SetFrameId",
boost::bind(&InteractiveMarkerViewer::SetFrameIdCommand, this, _1, _2),
"Set the frame in which interactive markers will be published."
);

set_environment(env);
}
Expand Down Expand Up @@ -184,7 +192,7 @@ void InteractiveMarkerViewer::EnvironmentSync()
env_->GetBodies(bodies);

for (KinBodyPtr const &body : bodies) {
OpenRAVE::UserDataPtr raw = body->GetUserData("interactive_marker");
OpenRAVE::UserDataPtr raw = body->GetUserData("interactive_marker");
auto body_marker = boost::dynamic_pointer_cast<KinBodyMarker>(raw);

// It's possibe to get here without the body's KinBodyMarker being
Expand All @@ -193,7 +201,7 @@ void InteractiveMarkerViewer::EnvironmentSync()
if (!body_marker) {
BodyCallback(body, 1);

raw = body->GetUserData("interactive_marker");
raw = body->GetUserData("interactive_marker");
body_marker = boost::dynamic_pointer_cast<KinBodyMarker>(raw);
BOOST_ASSERT(body_marker);
}
Expand Down Expand Up @@ -485,7 +493,7 @@ bool InteractiveMarkerViewer::AddMenuEntryCommand(std::ostream &out,
);
}

OpenRAVE::UserDataPtr const marker_raw = kinbody->GetUserData("interactive_marker");
OpenRAVE::UserDataPtr const marker_raw = kinbody->GetUserData("interactive_marker");
auto const marker = boost::dynamic_pointer_cast<KinBodyMarker>(marker_raw);
if (!marker) {
throw OpenRAVE::openrave_exception(
Expand Down Expand Up @@ -566,6 +574,30 @@ bool InteractiveMarkerViewer::GetMenuSelectionCommand(std::ostream &out,
out << menu_queue_.rdbuf();
}

bool GetFrameIdCommand(std::ostream &out, std::istream &in)
{
out << parent_frame_id_;
}

bool SetFrameIdCommand(std::ostream &out, std::istream &in)
{
std::string new_frame_id;
in >> new_frame_id;

// We should not publish an empty frame ID.
if (new_frame_id.empty()) {
throw OpenRAVE::openrave_exception(
"The frame ID cannot be empty.",
OpenRAVE::ORE_Failed
);
}

// Set the frame ID used for all marker publications.
RAVELOG_DEBUG("Frame ID changed: '%s' -> '%s'\n",
parent_frame_id_.c_str(), new_frame_id);
set_parent_frame(new_frame_id);
}

void InteractiveMarkerViewer::BodyCallback(OpenRAVE::KinBodyPtr body, int flag)
{
RAVELOG_DEBUG("BodyCallback %s -> %d\n", body->GetName().c_str(), flag);
Expand Down