Go to the documentation of this file.00001 #include "wam_cartesian_planning_alg_node.h"
00002
00003 WamCartesianPlanningAlgNode::WamCartesianPlanningAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<WamCartesianPlanningAlgorithm>()
00005 {
00006
00007
00008
00009
00010
00011
00012
00013
00014 this->path_planning_server_ = this->public_node_handle_.advertiseService("path_planning", &WamCartesianPlanningAlgNode::path_planningCallback, this);
00015
00016
00017
00018
00019
00020
00021 }
00022
00023 WamCartesianPlanningAlgNode::~WamCartesianPlanningAlgNode(void)
00024 {
00025
00026 }
00027
00028 void WamCartesianPlanningAlgNode::mainNodeThread(void)
00029 {
00030
00031
00032
00033
00034
00035
00036
00037 }
00038
00039
00040
00041
00042 bool WamCartesianPlanningAlgNode::path_planningCallback(iri_wam_cartesian_planning::PosePath::Request &req, iri_wam_cartesian_planning::PosePath::Response &res)
00043 {
00044 ROS_INFO("WamCartesianPlanningAlgNode::path_planningCallback: New Request Received!");
00045 std::vector<geometry_msgs::PoseStamped> vector;
00046
00047 this->alg_.lock();
00048 ompl::base::ScopedState<ompl::base::SE3StateSpace> st1 =rosToOmpl(req.init_pose);
00049 ompl::base::ScopedState<ompl::base::SE3StateSpace> st2 =rosToOmpl(req.goal_pose);
00050 this->alg_.planWithSimpleSetup(st1,st2,req.states,vector);
00051 res.poses_solution=vector;
00052 res.solution_found=(vector.size() >0)? true: false;
00053 this->alg_.unlock();
00054 return true;
00055 }
00056
00057
00058
00059
00060
00061 void WamCartesianPlanningAlgNode::node_config_update(Config &config, uint32_t level)
00062 {
00063 this->alg_.lock();
00064
00065 this->alg_.unlock();
00066 }
00067
00068 void WamCartesianPlanningAlgNode::addNodeDiagnostics(void)
00069 {
00070 }
00071
00072 ompl::base::ScopedState<ompl::base::SE3StateSpace> WamCartesianPlanningAlgNode::rosToOmpl(const geometry_msgs::PoseStamped& msg)
00073 {
00074 ompl::base::ScopedState<ompl::base::SE3StateSpace> st(this->alg_.space);
00075 st->setXYZ(msg.pose.position.x,msg.pose.position.y,msg.pose.position.z);
00076 st->rotation().x=msg.pose.orientation.x;
00077 st->rotation().y=msg.pose.orientation.y;
00078 st->rotation().z=msg.pose.orientation.z;
00079 st->rotation().w=msg.pose.orientation.w;
00080 return st;
00081 }
00082
00083
00084 int main(int argc,char *argv[])
00085 {
00086 return algorithm_base::main<WamCartesianPlanningAlgNode>(argc, argv, "wam_cartesian_planning_alg_node");
00087 }