MarkerController::MarkerController(std::string goal_topic_name, std::string int_server_name){
ros::NodeHandle node;
goal_pub = node.advertise<geometry_msgs::PoseStamped>(goal_topic_name, 1);
server.reset( new interactive_markers::InteractiveMarkerServer(int_server_name,"",false) );
menu_handler.insert("Select Goal",boost::bind(&MarkerController::processFeedback,this,_1));
makeViewFacingMarker();
server->applyChanges();
}
StigmergyPlanner::StigmergyPlanner():starting_pose_ready(false),drawn_path(false),finished(false)
{
path_pub = node.advertise<nav_msgs::Path>("/stigmergy_path",1);
final_path_pub = node.advertise<nav_msgs::Path>("/final_stigmergy_path",1);
target_pub = node.advertise<visualization_msgs::Marker>("/target",1);
server.reset( new interactive_markers::InteractiveMarkerServer("stigmergy_planner","",false) );
menu_handler.insert("Select Starting Pose",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Undo Starting Pose",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Apply Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Discard Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Undo Planning",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Evaluate Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
while(!getRobotPose(robot_pose))
{
ROS_INFO("Waiting for transformation");
}
makeViewFacingMarker(robot_pose);
server->applyChanges();
target.header.frame_id = "/map";
target.header.stamp = ros::Time::now();
target.id = 1;
target.ns = "target";
target.scale.x = 0.3;
target.scale.y = 0.3;
target.scale.z = 0.3;
target.type = visualization_msgs::Marker::MESH_RESOURCE;
target.mesh_resource = "package://stigmergy_planner/meshes/barrier.dae";
target.mesh_use_embedded_materials = true;
target.lifetime = ros::Duration(0);
}
