BlockLogicServer(const std::string name) :
nh_("~"),
interactive_m_server_("block_controls"),
action_server_(name, false),
action_name_(name),
initialized_(false),
block_size(0.4)
{
// Load parameters from the server.
nh_.param<double>("bump_size", bump_size, 0.0); // original 0.005
// ---------------------------------------------------------------------------------------------
// Read in all seen blocks
block_sub_ = nh_.subscribe("/cube_block_poses", 1, &BlockLogicServer::addBlocks, this);
// ---------------------------------------------------------------------------------------------
// Publish location of blocks??
pick_place_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/pick_place", 1, true);
// ---------------------------------------------------------------------------------------------
// Rviz Visualizations
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// ---------------------------------------------------------------------------------------------
// Register the goal and feeback callbacks for action server
action_server_.registerGoalCallback(boost::bind(&BlockLogicServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&BlockLogicServer::preemptCB, this));
action_server_.start();
}
PickAndPlaceServer(const std::string name) :
nh_("~"), as_(name, false), action_name_(name), client_("/move_arm", true)
{
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
as_.start();
}
MovePlatformAction() :
as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
ac_planner_("/plan_action", true), // Planner action CLIENT
ac_control_("/control_action", true) // Controller action CLIENT
{
n_.param("/move_platform_server/debug", debug_, false);
std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
name = name + ".debug";
logger_name_ = "debug";
//logger is ros.carlos_motion_action_server.debug
if (debug_)
{
// if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
// so for debug we'll use a named logger
if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
ros::console::notifyLoggerLevelsChanged();
}
else // if not DEBUG we want INFO
{
if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
ros::console::notifyLoggerLevelsChanged();
}
ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");
as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));
//start the move server
as_.start();
ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");
// now wait for the other servers (planner + controller) to start
ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
ac_planner_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected());
ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
ac_control_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected());
n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);
planning_ = false;
controlling_ = false;
//set_terminal_state_;
ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);
}
ApproachBucketAction(std::string name) :
as_(nh_, name, false),
action_name_(name)
{
// register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&ApproachBucketAction::goalCB, this));
as_.registerPreemptCallback(boost::bind(&ApproachBucketAction::preemptCB, this));
twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
us_sub_ = nh_.subscribe("/obstacle", 1, &ApproachBucketAction::ultraSonicsCB, this);
point_sub_ = nh_.subscribe("track_bucket_point_avg", 1, &ApproachBucketAction::trackPointCB, this);
area_sub_ = nh_.subscribe("track_bucket_area", 1, &ApproachBucketAction::trackAreaCB, this);
obstacle_detected_=false;
drop_flag = false;
as_.start();
}
PickAndPlaceServer(const std::string name) :
nh_("~"), as_(name, false), action_name_(name), client_("/move_right_arm", true)
{
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
as_.start();
client_.waitForServer();
ROS_INFO("Connected to server");
gripper = nh_.advertise<std_msgs::Float64>("/parallel_gripper_controller/command", 1, false);
}
DynamixelProActionServer(std::string name): _actionServer(_nodeHandle, name, boost::bind(&DynamixelProActionServer::goalCallback, this, _1) ,false) {
_actionServer.registerPreemptCallback(boost::bind(&DynamixelProActionServer::preemptCallback, this));
if(_nodeHandle.getParam("joint_name_list", _jointInfo.name)) {
init();
_publishFeedback = false;
_jointState = _nodeHandle.subscribe("joint_states", 100, &DynamixelProActionServer::jointStateCallback,
this);
_jointCommand = _nodeHandle.advertise<sensor_msgs::JointState>("joint_commands", 10);
_actionServer.start();
rosInfo("Server is active");
}
else {
rosError("Cloud not find joint_name_list parameter");
ros::shutdown();
}
}
MoveActionServer(const std::string name) :
nh_("~"), as_(nh_, name, false), action_name_(name)
{
// Get parameters
nh_.param<std::string>("base_frame", base_frame, "base_link");
nh_.param<std::string>("odom_frame", odom_frame, "odom");
nh_.param<double>("turn_rate", turn_rate, 0.75);
nh_.param<double>("forward_rate", forward_rate, 0.25);
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&MoveActionServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&MoveActionServer::preemptCB, this));
as_.start();
cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
Explore(std::string name):
as_(nh_, name, false),
action_name_(name),
ac_("move_base", true)
{
explore_state_ = STOP;
explore_ = false;
firstGoalSend = false;
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));
as_.start();
alghoritm_state_sub_ = nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);
}
Explore(std::string name):
as_(nh_, name, false),
action_name_(name),
ac_("move_base", true)
{
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));
as_.start();
firstGoalSend = false;
//all_balls = nh_.subscribe < geometry_msgs::PoseArray > ("allBalls", 10, &ChooseAccessibleBalls::allBallsCb, this);
alghoritm_state_sub_ = nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);
explore_state_ = STOP;
explore_ = false;
}
// Constructor
PickPlaceMoveItServer(const std::string name):
nh_("~"),
movegroup_action_("pickup", true),
clam_arm_client_("clam_arm", true),
ee_marker_is_loaded_(false),
block_published_(false),
action_server_(name, false)
{
base_link_ = "/base_link";
// -----------------------------------------------------------------------------------------------
// Adding collision objects
collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1);
// -----------------------------------------------------------------------------------------------
// Connect to move_group/Pickup action server
while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server");
}
// ---------------------------------------------------------------------------------------------
// Connect to ClamArm action server
while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server");
}
// ---------------------------------------------------------------------------------------------
// Create planning scene monitor
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));
if (planning_scene_monitor_->getPlanningScene())
{
//planning_scene_monitor_->startWorldGeometryMonitor();
//planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
//planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
}
else
{
ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured");
}
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_));
// ---------------------------------------------------------------------------------------------
// Register the goal and preempt callbacks
action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this));
action_server_.start();
// Announce state
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready.");
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command...");
// ---------------------------------------------------------------------------------------------
// Send home
ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home");
clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET;
clam_arm_client_.sendGoal(clam_arm_goal_);
while(!clam_arm_client_.getState().isDone() && ros::ok())
ros::Duration(0.1).sleep();
// ---------------------------------------------------------------------------------------------
// Send fake command
fake_goalCB();
}
HueDetector3DServer(std::string s_name) :
server_(n_, s_name), action_name_(s_name) {
first = true;
if (!n_.getParam("HueServer/maxNumComponents", maxNumComponents)) {
ROS_INFO("Could not retrieve 'maxNumComponents' parameter, setting to default value of '%d'", MAX_FEATURES);
maxNumComponents = MAX_FEATURES;
}
if (!n_.getParam("HueServer/blurKernel", blurKernel)) {
ROS_INFO("Could not retrieve 'blurKernel' parameter, setting to default value of '%d'", init_blur_kernel);
blurKernel = init_blur_kernel;
}
if (!n_.getParam("HueServer/minCCThreshold", minCCThreshold)) {
ROS_INFO("Could not retrieve 'minCCThreshold' parameter, setting to default value of '%d'", DEFAULT_MIN_CC_THRESHOLD);
minCCThreshold = DEFAULT_MIN_CC_THRESHOLD;
}
if (!n_.getParam("HueServer/maxCCThreshold", maxCCThreshold)) {
ROS_INFO("Could not retrieve 'maxCCThreshold' parameter, setting to default value of '%d'", DEFAULT_MAX_CC_THRESHOLD);
maxCCThreshold = DEFAULT_MAX_CC_THRESHOLD;
}
if (!n_.getParam("HueServer/display_image", display_image)) {
ROS_INFO("Could not retrieve 'display_image' parameter, setting to default value of '%d'", init_display_image);
display_image = init_display_image;
}
std::string source_identifier;
if (!n_.getParam("HueServer/source_identifier", source_identifier)) {
ROS_INFO("Could not retrieve 'source_identifier' parameter, setting to default value of '%s'", DEFAULT_SOURCE_IDENTIFIER);
source_identifier = DEFAULT_SOURCE_IDENTIFIER;
}
std::stringstream hue_name;
hue_name << name << "HueServer";
sprintf(pointcloud_topic, "%s", source_identifier.c_str()); //input topic
sprintf(color_topic, "%s/Feature/ColorBlobs/%s", source_identifier.c_str(), hue_name.str().c_str());
srand ( time(NULL) );
//output topic
sprintf(marker_topic,"ellipsoid_marker/%s",hue_name.str().c_str());
printf("Topic name : %s\n", color_topic);
pointcloud_sub_ = n_.subscribe(pointcloud_topic, 1, &HueDetector3DServer::pointCloudCallback, this);
//moments_pub_ = n_.advertise<Image_msgs::FeatureMoments3D>(color_topic, 1);
object_pose_marker_pub_ = n_.advertise<visualization_msgs::Marker>(marker_topic,1);
if (display_image) {
namedWindow(name, CV_WINDOW_AUTOSIZE);
//namedWindow("feat", CV_WINDOW_AUTOSIZE);
createTrackbar("Blur Kernel", name, &blurKernel, 25, &on_trackbar);
createTrackbar("Min Size", name, &minCCThreshold, 3000, &on_trackbar);
createTrackbar("Max Size", name, &maxCCThreshold, 10000, &on_trackbar);
}
hue_image = new Mat(height, width, CV_8UC1);
back_img = new Mat(hue_image->size(), CV_8UC1);
temp_blurred_image = new Mat(hue_image->size(), CV_8UC1);
blurred_image = new Mat(hue_image->size(), CV_8UC1);
flipped = new Mat(hue_image->size(), CV_8UC1);
rgb_image = new Mat(hue_image->size(), CV_8UC3);
hsv_image = new Mat(hue_image->size(), CV_8UC3);
is_running = false;
got_goals = false;
num_feature_goals_ = 0;
timer_ = n_.createTimer(ros::Duration(1.0 / update_rate), &HueDetector3DServer::Update, this);
//register the goal and feeback callbacks
server_.registerGoalCallback(boost::bind(&HueDetector3DServer::GoalCB, this));
server_.registerPreemptCallback(boost::bind(&HueDetector3DServer::PreemptCB, this));
ROS_INFO("Hue Server started ");
}
