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

Add description parameter to plotTrajectory functions #94

Merged
merged 1 commit into from
Feb 22, 2024
Merged

Add description parameter to plotTrajectory functions #94

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
15 changes: 12 additions & 3 deletions tesseract_rosutils/include/tesseract_rosutils/plotting.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,15 +61,24 @@ class ROSPlotting : public tesseract_visualization::Visualization
const tesseract_scene_graph::StateSolver& state_solver,
std::string ns = "") override;

void plotTrajectory(const tesseract_msgs::msg::Trajectory& traj, std::string ns = "");
void plotTrajectory(const tesseract_common::JointTrajectory& traj, std::string ns = "", std::string description = "");

void plotTrajectory(const tesseract_msgs::msg::Trajectory& traj);

void plotTrajectory(const tesseract_environment::Environment& env,
const tesseract_planning::InstructionPoly& instruction,
std::string ns = "");
std::string ns = "",
std::string description = "");

void plotTrajectories(const tesseract_environment::Environment& env,
const std::vector<tesseract_planning::InstructionPoly>& instructions,
std::string ns,
std::string description);

void plotTrajectory(const tesseract_environment::Commands& cmds,
const tesseract_planning::InstructionPoly& instruction,
std::string = "");
std::string = "",
std::string description = "");

void plotToolpath(const tesseract_environment::Environment& env,
const tesseract_planning::InstructionPoly& instruction,
Expand Down
61 changes: 38 additions & 23 deletions tesseract_rosutils/src/plotting.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,25 +97,24 @@ void ROSPlotting::waitForConnection(long seconds) const

rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.02)));
}

return;
}

void ROSPlotting::plotEnvironment(const tesseract_environment::Environment& /*env*/, std::string /*ns*/) {}

void ROSPlotting::plotEnvironmentState(const tesseract_scene_graph::SceneState& /*state*/, std::string /*ns*/) {}

void ROSPlotting::plotTrajectory(const tesseract_msgs::msg::Trajectory& traj, std::string /*ns*/)
{
trajectory_pub_->publish(traj);
}

void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
const tesseract_scene_graph::StateSolver& /*state_solver*/,
std::string ns)
{
plotTrajectory(traj, ns);
}

void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj, std::string ns, std::string description)
{
tesseract_msgs::msg::Trajectory msg;
msg.ns = ns;
msg.ns = std::move(ns);
msg.description = std::move(description);

if (!traj.empty())
{
Expand All @@ -137,12 +136,24 @@ void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
plotTrajectory(msg);
}

void ROSPlotting::plotTrajectory(const tesseract_msgs::msg::Trajectory& traj) { trajectory_pub_->publish(traj); }

void ROSPlotting::plotTrajectory(const tesseract_environment::Environment& env,
const tesseract_planning::InstructionPoly& instruction,
std::string ns)
std::string ns,
std::string description)
{
plotTrajectories(env, { instruction }, std::move(ns), std::move(description));
}

void ROSPlotting::plotTrajectories(const tesseract_environment::Environment& env,
const std::vector<tesseract_planning::InstructionPoly>& instructions,
std::string ns,
std::string description)
{
tesseract_msgs::msg::Trajectory msg;
msg.ns = ns;
msg.ns = std::move(ns);
msg.description = std::move(description);

// Set tesseract state information
toMsg(msg.environment, env);
Expand All @@ -157,25 +168,29 @@ void ROSPlotting::plotTrajectory(const tesseract_environment::Environment& env,
msg.initial_state.push_back(pair_msg);
}

// Convert to joint trajectory
assert(instruction.isCompositeInstruction());
const auto& ci = instruction.as<tesseract_planning::CompositeInstruction>();
tesseract_common::JointTrajectory traj = tesseract_planning::toJointTrajectory(ci);

// Set the joint trajectory message
tesseract_msgs::msg::JointTrajectory traj_msg;
toMsg(traj_msg, traj);
msg.joint_trajectories.push_back(traj_msg);
// Convert to joint trajectories
for (const auto& instruction : instructions)
{
assert(instruction.isCompositeInstruction());
const auto& ci = instruction.as<tesseract_planning::CompositeInstruction>();
tesseract_common::JointTrajectory traj = tesseract_planning::toJointTrajectory(ci);
// Set the joint trajectory message
tesseract_msgs::msg::JointTrajectory traj_msg;
toMsg(traj_msg, traj);
msg.joint_trajectories.push_back(traj_msg);
}

plotTrajectory(msg);
}

void ROSPlotting::plotTrajectory(const tesseract_environment::Commands& cmds,
const tesseract_planning::InstructionPoly& instruction,
std::string ns)
std::string ns,
std::string description)
{
tesseract_msgs::msg::Trajectory msg;
msg.ns = ns;
msg.ns = std::move(ns);
msg.description = std::move(description);

// Set the commands message
std::vector<tesseract_msgs::msg::EnvironmentCommand> env_cmds;
Expand Down Expand Up @@ -308,20 +323,20 @@ visualization_msgs::msg::MarkerArray ROSPlotting::getMarkerAxisMsg(int& id_count

void ROSPlotting::clear(std::string ns)
{
// Remove old arrows
// Remove old markers
marker_counter_ = 0;
visualization_msgs::msg::MarkerArray msg;
visualization_msgs::msg::Marker marker;
marker.header.frame_id = root_link_;
marker.header.stamp = rclcpp::Time();
marker.ns = ns;
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::ARROW;
marker.action = visualization_msgs::msg::Marker::DELETEALL;
msg.markers.push_back(marker);
collisions_pub_->publish(msg);
arrows_pub_->publish(msg);
axes_pub_->publish(msg);
tool_path_pub_->publish(msg);
}

static void waitForInputAsync(std::string message)
Expand Down
Loading