trajectory_visualization.cpp
Go to the documentation of this file.
00001 
00026 #include <moveit/robot_state/conversions.h>
00027 #include <tf/transform_datatypes.h>
00028 #include <pluginlib/class_list_macros.h>
00029 #include <stomp_moveit/update_filters/trajectory_visualization.h>
00030 
00031 PLUGINLIB_EXPORT_CLASS(stomp_moveit::update_filters::TrajectoryVisualization,stomp_moveit::update_filters::StompUpdateFilter);
00032 
00033 
00034 typedef std::vector<geometry_msgs::Point> ToolLine;
00035 using namespace moveit::core;
00036 
00037 static const int MARKER_ID = 1;
00038 
00039 inline void eigenToPointsMsgs(const Eigen::MatrixXd& in,std::vector<geometry_msgs::Point>& out)
00040 {
00041   // resizing
00042   if(out.size()!= in.cols())
00043   {
00044     out.resize(in.cols());
00045   }
00046 
00047   // copying points
00048   for(auto t = 0u; t < in.cols(); t++)
00049   {
00050     out[t].x = in(0,t);
00051     out[t].y = in(1,t);
00052     out[t].z = in(2,t);
00053   }
00054 }
00055 
00062 static Eigen::MatrixXd jointsToToolPath(RobotState& state,const std::string& group_name,const Eigen::MatrixXd& parameters)
00063 {
00064 
00065   Eigen::MatrixXd tool_traj = Eigen::MatrixXd::Zero(3,parameters.cols());
00066   const moveit::core::JointModelGroup* joint_group = state.getJointModelGroup(group_name);
00067   std::string tool_link = joint_group->getLinkModelNames().back();
00068   for(auto t = 0u; t < parameters.cols();t++)
00069   {
00070     state.setJointGroupPositions(joint_group,parameters.col(t));
00071     Eigen::Affine3d tool_pos = state.getFrameTransform(tool_link);
00072     tool_traj(0,t) = tool_pos.translation()(0);
00073     tool_traj(1,t) = tool_pos.translation()(1);
00074     tool_traj(2,t) = tool_pos.translation()(2);
00075   }
00076 
00077   return std::move(tool_traj);
00078 }
00079 
00080 inline void createToolPathMarker(const Eigen::MatrixXd& tool_line, int id, std::string frame_id,
00081                           const std_msgs::ColorRGBA& rgb,double line_width,
00082                           std::string ns,visualization_msgs::Marker& m)
00083 {
00084   m.ns = ns;
00085   m.id = id;
00086   m.header.frame_id = frame_id;
00087   m.type = m.LINE_STRIP;
00088   m.action = m.ADD;
00089   m.color = rgb;
00090   tf::poseTFToMsg(tf::Transform::getIdentity(),m.pose);
00091   m.scale.x = line_width;
00092 
00093   if(tool_line.cols() == 0)
00094   {
00095     return;
00096   }
00097 
00098   // copying points into marker
00099   eigenToPointsMsgs(tool_line,m.points);
00100 }
00101 
00102 namespace stomp_moveit
00103 {
00104 namespace update_filters
00105 {
00106 
00107 TrajectoryVisualization::TrajectoryVisualization():
00108     name_("TrajectoryVisualization"),
00109     nh_("~"),
00110     line_width_(0.0),
00111     publish_intermediate_(false)
00112 {
00113   // TODO Auto-generated constructor stub
00114 
00115 }
00116 
00117 TrajectoryVisualization::~TrajectoryVisualization()
00118 {
00119   // TODO Auto-generated destructor stub
00120 }
00121 
00122 bool TrajectoryVisualization::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
00123                         const std::string& group_name,const XmlRpc::XmlRpcValue& config)
00124 {
00125   robot_model_ = robot_model_ptr;
00126   group_name_ = group_name;
00127 
00128   if(!configure(config))
00129   {
00130     return false;
00131   }
00132 
00133   // initializing publisher
00134   viz_pub_ = nh_.advertise<visualization_msgs::Marker>(marker_topic_,1);
00135 
00136   return true;
00137 }
00138 
00139 bool TrajectoryVisualization::configure(const XmlRpc::XmlRpcValue& config)
00140 {
00141 
00142   auto toColorRgb = [](XmlRpc::XmlRpcValue& p)
00143   {
00144     std_msgs::ColorRGBA rgb;
00145     rgb.r = (static_cast<int>(p[0]))/255.0;
00146     rgb.g = static_cast<int>(p[1])/255.0;
00147     rgb.b = static_cast<int>(p[2])/255.0;
00148     rgb.a = 1.0;
00149     return rgb;
00150   };
00151 
00152   // check parameter presence
00153   auto members = {"line_width","rgb","error_rgb","publish_intermediate","marker_topic","marker_namespace"};
00154   for(auto& m : members)
00155   {
00156     if(!config.hasMember(m))
00157     {
00158       ROS_ERROR("%s failed to find one or more required parameters",getName().c_str());
00159       return false;
00160     }
00161   }
00162 
00163   XmlRpc::XmlRpcValue c = config;
00164   try
00165   {
00166     line_width_ = static_cast<double>(c["line_width"]);
00167     rgb_ = toColorRgb(c["rgb"]);
00168     error_rgb_ = toColorRgb(c["error_rgb"]);
00169     publish_intermediate_ = static_cast<bool>(c["publish_intermediate"]);
00170     marker_topic_ = static_cast<std::string>(c["marker_topic"]);
00171     marker_namespace_ = static_cast<std::string>(c["marker_namespace"]);
00172   }
00173   catch(XmlRpc::XmlRpcException& e)
00174   {
00175     ROS_ERROR("%s failed to load required parameters, %s",getName().c_str(),e.getMessage().c_str());
00176     return false;
00177   }
00178 
00179   return true;
00180 }
00181 
00182 bool TrajectoryVisualization::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00183                  const moveit_msgs::MotionPlanRequest &req,
00184                  const stomp_core::StompConfiguration &config,
00185                  moveit_msgs::MoveItErrorCodes& error_code)
00186 {
00187   using namespace moveit::core;
00188 
00189   error_code.val = error_code.SUCCESS;
00190 
00191 
00192   // initializing points array
00193   tool_traj_line_ = Eigen::MatrixXd::Zero(3,config.num_timesteps);
00194 
00195   // initializing marker
00196   createToolPathMarker(tool_traj_line_,
00197                        MARKER_ID,robot_model_->getRootLinkName(),
00198                        rgb_,line_width_,
00199                        marker_namespace_,tool_traj_marker_);
00200 
00201   // updating state
00202   state_.reset(new RobotState(robot_model_));
00203   if(!robotStateMsgToRobotState(req.start_state,*state_,true))
00204   {
00205     ROS_ERROR("%s Failed to get current robot state from request",getName().c_str());
00206     return false;
00207   }
00208 
00209   //delete current marker
00210   visualization_msgs::Marker m;
00211   createToolPathMarker(Eigen::MatrixXd(),MARKER_ID,robot_model_->getRootLinkName(),rgb_,line_width_,marker_namespace_,m);
00212   m.action = m.DELETE;
00213   viz_pub_.publish(m);
00214 
00215   return true;
00216 }
00217 
00218 bool TrajectoryVisualization::filter(std::size_t start_timestep,
00219                                      std::size_t num_timesteps,
00220                                      int iteration_number,
00221                                      const Eigen::MatrixXd& parameters,
00222                                      Eigen::MatrixXd& updates,
00223                                      bool& filtered)
00224 {
00225 
00226   if(!state_)
00227   {
00228     ROS_ERROR("%s Robot State has not been updated",getName().c_str());
00229     return false;
00230   }
00231 
00232   if(publish_intermediate_)
00233   {
00234     Eigen::MatrixXd updated_parameters = parameters + updates;
00235     tool_traj_line_ = jointsToToolPath(*state_,group_name_,updated_parameters);
00236     eigenToPointsMsgs(tool_traj_line_,tool_traj_marker_.points);
00237     viz_pub_.publish(tool_traj_marker_);
00238   }
00239 
00240   return true;
00241 }
00242 
00243 void TrajectoryVisualization::done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters)
00244 {
00245 
00246   tool_traj_line_ = jointsToToolPath(*state_,group_name_,parameters);
00247   eigenToPointsMsgs(tool_traj_line_,tool_traj_marker_.points);
00248 
00249   if(!success)
00250   {
00251     tool_traj_marker_.color = error_rgb_;
00252   }
00253 
00254   viz_pub_.publish(tool_traj_marker_);
00255 }
00256 
00257 } /* namespace updated_filters */
00258 } /* namespace stomp_moveit */


stomp_moveit
Author(s): Jorge Nicho
autogenerated on Sat Jun 8 2019 19:24:01