example setting solution markers

This commit is contained in:
Robert Haschke 2017-11-29 11:22:29 +01:00
parent 97bb6ef2c6
commit 7a13d79bc4
5 changed files with 91 additions and 14 deletions

View File

@ -82,7 +82,7 @@ int main(int argc, char** argv){
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
"lh_tool_frame");
gengrasp->setAngleDelta(-.2);
gengrasp->setAngleDelta(M_PI / 10.);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT);
@ -113,7 +113,9 @@ int main(int argc, char** argv){
try {
t.plan();
t.publishAllSolutions();
std::cout << "waiting for <enter>\n";
char ch;
std::cin >> ch;
}
catch (const InitStageException &e) {
std::cerr << e;

View File

@ -110,8 +110,16 @@ int main(int argc, char** argv){
t.add(std::move(move));
}
t.plan();
t.publishAllSolutions();
try {
t.plan();
std::cout << "waiting for <enter>\n";
char ch;
std::cin >> ch;
}
catch (const InitStageException &e) {
std::cerr << e;
return EINVAL;
}
return 0;
}

View File

@ -110,8 +110,16 @@ int main(int argc, char** argv){
t.add(std::move(move));
}
t.plan();
t.publishAllSolutions();
try {
t.plan();
std::cout << "waiting for <enter>\n";
char ch;
std::cin >> ch;
}
catch (const InitStageException &e) {
std::cerr << e;
return EINVAL;
}
return 0;
}

View File

@ -3,6 +3,9 @@
//
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
@ -11,6 +14,7 @@
#include <eigen_conversions/eigen_msg.h>
#include <chrono>
#include <functional>
#include <iterator>
#include <ros/console.h>
namespace moveit { namespace task_constructor { namespace stages {
@ -213,14 +217,39 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
if (succeeded) {
// create a new scene for each solution as they will have different robot states
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
std::unique_ptr<SolutionBase> solution(new SubTrajectory());
spawn(InterfaceState(scene), s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
} else if (max_ik_solutions == 1)
// include markers from original solution
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers()));
// frame at target pose
target_pose_msg.header.frame_id = scene->getPlanningFrame();
rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame");
if (succeeded) {
solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
// create a new scene for each solution as they will have different robot states
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst();
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
scene = new_scene;
// robot model
robot_state.updateLinkTransforms();
auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) {
marker.ns = "ik solution";
marker.color.a *= 0.5;
solution->markers().push_back(marker);
};
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
} else {
solution->setCost(std::numeric_limits<double>::infinity());
}
spawn(InterfaceState(scene), std::move(solution));
if (!succeeded && max_ik_solutions == 1)
break; // first and only attempt failed
}
}

View File

@ -35,6 +35,9 @@
/* Authors: Robert Haschke, Michael Goerner */
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <rviz_marker_tools/marker_creation.h>
#include <moveit/planning_scene/planning_scene.h>
@ -143,7 +146,34 @@ bool GenerateGraspPose::compute(){
tf::poseEigenToMsg(link_pose, goal_pose_msg.pose);
state.properties().set("target_pose", goal_pose_msg);
spawn(std::move(state), 0.0);
SubTrajectory trajectory;
trajectory.setCost(0.0);
trajectory.setName(std::to_string(current_angle_));
// add an arrow marker
visualization_msgs::Marker m;
m.header.frame_id = scene_->getPlanningFrame();
m.ns = "grasp pose";
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
double scale = 0.1;
rviz_marker_tools::makeArrow(m, scale);
tf::poseEigenToMsg(grasp_pose * grasp2tool *
// arrow should point along z (instead of x)
Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitY()) *
// arrow tip at goal_pose
Eigen::Translation3d(-scale, 0, 0), m.pose);
trajectory.markers().push_back(m);
// add end-effector marker
robot_state.updateStateWithLinkAt(link_name, link_pose);
auto appender = [&trajectory](visualization_msgs::Marker& marker, const std::string& name) {
marker.ns = "grasp eef";
marker.color.a *= 0.5;
trajectory.markers().push_back(marker);
};
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
spawn(std::move(state), std::move(trajectory));
return true;
}