mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
bug fixes
- only attempt to retrieve solution if remote was not yet destroyed - setting get_solution_client is neccessary only once (pointer never changes) - more reliable topic handling + status feedback
This commit is contained in:
parent
3e788084b2
commit
5089de890f
@ -350,7 +350,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
|
|||||||
// to avoid some communication overhead
|
// to avoid some communication overhead
|
||||||
|
|
||||||
DisplaySolutionPtr result;
|
DisplaySolutionPtr result;
|
||||||
if (get_solution_client_) {
|
if (!(flags_ & IS_DESTROYED) && get_solution_client_) {
|
||||||
// request solution via service
|
// request solution via service
|
||||||
moveit_task_constructor_msgs::GetSolution srv;
|
moveit_task_constructor_msgs::GetSolution srv;
|
||||||
srv.request.solution_id = id;
|
srv.request.solution_id = id;
|
||||||
|
|||||||
@ -58,6 +58,8 @@ namespace moveit_rviz_plugin
|
|||||||
TaskDisplay::TaskDisplay() : Display()
|
TaskDisplay::TaskDisplay() : Display()
|
||||||
{
|
{
|
||||||
task_list_model_.reset(new TaskListModel);
|
task_list_model_.reset(new TaskListModel);
|
||||||
|
task_list_model_->setSolutionClient(&get_solution_client);
|
||||||
|
|
||||||
MetaTaskListModel::instance().insertModel(task_list_model_.get(), this);
|
MetaTaskListModel::instance().insertModel(task_list_model_.get(), this);
|
||||||
|
|
||||||
connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)),
|
connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)),
|
||||||
@ -165,6 +167,7 @@ void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_co
|
|||||||
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
|
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||||
mainloop_jobs_.addJob([this, id, msg]() {
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||||
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
@ -174,6 +177,7 @@ void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_con
|
|||||||
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
|
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||||
mainloop_jobs_.addJob([this, id, msg]() {
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||||
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
@ -183,6 +187,7 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
|
|||||||
const moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
|
const moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
|
||||||
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||||
mainloop_jobs_.addJob([this, id, msg]() {
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||||
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
||||||
if (s) trajectory_visual_->showTrajectory(s, false);
|
if (s) trajectory_visual_->showTrajectory(s, false);
|
||||||
return;
|
return;
|
||||||
@ -210,14 +215,14 @@ void TaskDisplay::changedTaskSolutionTopic()
|
|||||||
tasks_property_->removeChildren();
|
tasks_property_->removeChildren();
|
||||||
|
|
||||||
// generate task monitoring topics from solution topic
|
// generate task monitoring topics from solution topic
|
||||||
std::string solution_topic = task_solution_topic_property_->getStdString();
|
const QString& solution_topic = task_solution_topic_property_->getString();
|
||||||
auto last_sep = solution_topic.find_last_of('/');
|
if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) {
|
||||||
if (last_sep == std::string::npos) {
|
setStatus(rviz::StatusProperty::Error, "Task Monitor",
|
||||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "invalid topic");
|
QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC));
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string base_ns = solution_topic.substr(0, last_sep+1);
|
std::string base_ns = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC));
|
||||||
|
|
||||||
// listen to task descriptions updates
|
// listen to task descriptions updates
|
||||||
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
||||||
@ -226,13 +231,18 @@ void TaskDisplay::changedTaskSolutionTopic()
|
|||||||
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
|
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
|
||||||
|
|
||||||
// listen to task solutions
|
// listen to task solutions
|
||||||
task_solution_sub = update_nh_.subscribe(solution_topic, 2, &TaskDisplay::taskSolutionCB, this);
|
task_solution_sub = update_nh_.subscribe(base_ns + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this);
|
||||||
|
|
||||||
// service to request solutions
|
// service to request solutions
|
||||||
get_solution_client = update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE);
|
get_solution_client = update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE);
|
||||||
task_list_model_->setSolutionClient(&get_solution_client);
|
|
||||||
|
|
||||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received");
|
||||||
|
}
|
||||||
|
|
||||||
|
void TaskDisplay::setSolutionStatus(bool ok)
|
||||||
|
{
|
||||||
|
if (ok) setStatus(rviz::StatusProperty::Ok, "Solution", "Ok");
|
||||||
|
else setStatus(rviz::StatusProperty::Warn, "Solution", "Retrieval failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last)
|
void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last)
|
||||||
|
|||||||
@ -85,6 +85,7 @@ public:
|
|||||||
virtual void onEnable();
|
virtual void onEnable();
|
||||||
virtual void onDisable();
|
virtual void onDisable();
|
||||||
void setName(const QString& name);
|
void setName(const QString& name);
|
||||||
|
void setSolutionStatus(bool ok);
|
||||||
|
|
||||||
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
||||||
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
|
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
|
||||||
|
|||||||
@ -156,15 +156,6 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene)
|
|||||||
|
|
||||||
void TaskListModel::setSolutionClient(ros::ServiceClient *client)
|
void TaskListModel::setSolutionClient(ros::ServiceClient *client)
|
||||||
{
|
{
|
||||||
if (!client || get_solution_client_ != client ||
|
|
||||||
get_solution_client_->getService() != client->getService()) {
|
|
||||||
// service client's address changed: invalidate existing client pointers
|
|
||||||
for (int row=0, end = rowCount(); row != end; ++row) {
|
|
||||||
RemoteTaskModel* task = dynamic_cast<RemoteTaskModel*>(getModel(index(row, 0)).first);
|
|
||||||
if (!task) continue;
|
|
||||||
task->setSolutionClient(nullptr);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
get_solution_client_ = client;
|
get_solution_client_ = client;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -231,8 +231,10 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
|||||||
|
|
||||||
d_ptr->locked_display_ = display;
|
d_ptr->locked_display_ = display;
|
||||||
TaskSolutionVisualization* vis = display->visualization();
|
TaskSolutionVisualization* vis = display->visualization();
|
||||||
|
const DisplaySolutionPtr& solution = task->getSolution(current);
|
||||||
|
display->setSolutionStatus(bool(solution));
|
||||||
vis->interruptCurrentDisplay();
|
vis->interruptCurrentDisplay();
|
||||||
vis->showTrajectory(task->getSolution(current), true);
|
vis->showTrajectory(solution, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected)
|
void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected)
|
||||||
@ -248,6 +250,7 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const
|
|||||||
display->clearMarkers();
|
display->clearMarkers();
|
||||||
for (const auto& index : selected_rows) {
|
for (const auto& index : selected_rows) {
|
||||||
const DisplaySolutionPtr &solution = task->getSolution(index);
|
const DisplaySolutionPtr &solution = task->getSolution(index);
|
||||||
|
display->setSolutionStatus(bool(solution));
|
||||||
display->showMarkers(solution);
|
display->showMarkers(solution);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user