explicit BTAction(std::string name) :
as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
action_name_(name)
{
// Starts the action server
as_.start();
}
OrientToLaserReadingAction(ros::NodeHandle nh, std::string name, std::string cmd_vel_topic, std::string linreg_service_name) :
as_(nh, name, boost::bind(&OrientToLaserReadingAction::executeActionCB, this, _1), false)
{
//as_.registerGoalCallback();
this->action_name_ = name;
this->service_name = linreg_service_name;
this->cmd_vel_topic = cmd_vel_topic;
nh_ = nh;
target_distance = TARGET_DISTANCE;
max_velocity = MAX_VELOCITY;
ROS_INFO("Register publisher");
cmd_pub = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1000); // 1000-element buffer for publisher
ROS_INFO("Create service clinet");
client = nh_.serviceClient<BaseScanLinearRegression>(service_name);
as_.start();
}
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();
}
Node()
: private_nh("~"),
kill_listener(boost::bind(&Node::killed_callback, this)),
actionserver(nh, "moveto", false),
disabled(false) {
fixed_frame = uf_common::getParam<std::string>(private_nh, "fixed_frame");
body_frame = uf_common::getParam<std::string>(private_nh, "body_frame");
limits.vmin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
limits.vmax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
limits.amin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amin_b");
limits.amax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amax_b");
limits.arevoffset_b = uf_common::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
limits.umax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "umax_b");
traj_dt = uf_common::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));
odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));
trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);
update_timer =
nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));
actionserver.start();
set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
"set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
}
followerAction(std::string name) :
action_(nh_, name, boost::bind(&followerAction::executeCB, this, _1), false),
action_name_(name)
{
action_.start();
ROS_INFO("Waiting for the Client to start the process");
}
TestAction(std::string name) :
as_(nh_, name, boost::bind(&TestAction::executeCB, this, _1), false),
action_name_(name)
{
twist_pub = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
dist_sub = nh_.subscribe("/ir_publish/sensors", 1, &TestAction::distanceCallback, this);
distance_lefts_front = init_side_limit_max;
distance_lefts_back = init_side_limit_max;
distance_front_left = init_front_limit_max;
distance_front_right = init_front_limit_max;
distance_rights_front = init_side_limit_max;
distance_rights_back = init_side_limit_max;
// Coresponding to the right decision side
side = 0;
flagReady = 0;
srand(time(NULL));
state = FRONT;
counter = 0;
as_.start();
}
MotorController(int hz) : ignore_twist(false), ignored_twist_cnt(0), Node(), hz(hz), wheel_left(hz), wheel_right(hz), v(0), w(0), updates_since_last_twist(0), idle(false), is_still(false), stop_action(nh, ACTION_STOP, boost::bind(&MotorController::action_execute_stop_callback, this, _1), false), is_stopping(false) {
init_params();
print_params();
pwm_publisher = nh.advertise<ras_arduino_msgs::PWM>(TOPIC_PWM, BUFFER_SIZE);
actual_twist_publisher = nh.advertise<geometry_msgs::Twist>(TOPIC_ACTUAL_TWIST, BUFFER_SIZE);
twist_subscriber = nh.subscribe<geometry_msgs::Twist>(TOPIC_TWIST, BUFFER_SIZE, &MotorController::twist_callback, this);
encoders_subscriber = nh.subscribe<ras_arduino_msgs::Encoders>(TOPIC_ENCODERS, BUFFER_SIZE, &MotorController::encoders_callback, this);
stop_action.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();
}
ExpressionDriver(std::string name, ros::NodeHandle &n) : nh_(n), nPriv("~"), as_(nh_, name, boost::bind(&ExpressionDriver::executeCB, this, _1), false),
action_name_()
{
//Initialize the port
nPriv.param<std::string>("port", port, "/dev/ttyACM0");
nPriv.param<int>("baudrate", baudrate, 115200);
try
{
driverComms = new DriverComms(port, baudrate);
}
catch(const std::exception&)
{
throw std::exception();
}
//Start the action server
as_.start();
//Initialize and set the default expression
bool sentM, sentS, sentR, sentL;
sentM = driverComms->sendCommand(ExpressionValues::ExpressionHappy::MOUTH, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_MOUTH);
feedback_.mouth_value = ExpressionValues::ExpressionHappy::MOUTH;
feedback_.mouth_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
sentS = driverComms->sendCommand(ExpressionValues::ExpressionHappy::EYELIDS, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_EYELIDS);
feedback_.eyelids_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.eyelids_value = ExpressionValues::ExpressionHappy::EYELIDS;
sentR = driverComms->sendCommand(ExpressionValues::ExpressionHappy::RIGHT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_RIGHTEYEBROW);
feedback_.rightEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.rightEyebrow_value = ExpressionValues::ExpressionHappy::RIGHT_EYEBROW;
sentL = driverComms->sendCommand(ExpressionValues::ExpressionHappy::LEFT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_LEFTEYEBROW);
feedback_.leftEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
feedback_.leftEyebrow_value = ExpressionValues::ExpressionHappy::LEFT_EYEBROW;
if(sentM && sentS && sentR && sentL)
{
ROS_INFO("Initialization commands sent. Vizzy is Happy! :D");
}
else
{
ROS_ERROR("Expression initialization failed. Check the board? :(");
as_.shutdown();
delete driverComms;
throw std::exception();
}
}
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);
}
SayAction(std::string name) :
as_(nh_, name, boost::bind(&SayAction::as_cb, this, _1), false),
action_name_(name)
{
as_.start();
srvServer_ = nh_.advertiseService("/say", &SayAction::service_cb, this);
srvServer_mute_ = nh_.advertiseService("mute", &SayAction::service_cb_mute, this);
srvServer_unmute_ = nh_.advertiseService("unmute", &SayAction::service_cb_unmute, this);
sub_ = nh_.subscribe("/say", 1000, &SayAction::topic_cb, this);
topicPub_Diagnostic_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
mute_ = false;
}
Trajectory_Tracker(ros::NodeHandle n) :
action_server(n,ros::this_node::getName(),boost::bind(&Trajectory_Tracker::track_trajectory, this, _1), false),
action_name(ros::this_node::getName())
{
action_server.start();
ROS_INFO("STARTED ACTION SERVER");
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
cmd_pub = n.advertise<geometry_msgs::Pose2D>("cmd", 1000);
Kp_x = 1;
Kp_y = 4;
Kp_w = 1.75;
}
SpinBucketAction(std::string name) :
as_(nh_, name, boost::bind(&SpinBucketAction::executeCB, this, _1), false),
action_name_(name)
{
// register the goal and feeback callbacks
point_sub_ = nh_.subscribe("/track_bucket_point_avg", 1, &SpinBucketAction::trackBucketPointCB, this);
ir_sub_ = nh_.subscribe("/beacon", 1, &SpinBucketAction::trackIRCB, this);
twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
bucket_detected_=false;
ir_detected_=false;
entered_ir = 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);
}
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();
}
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);
}
/**
* @brief This action server uses the softMotion-libs to move the robot
* along a main trajectory described in its goal.
* It correct errors by computing small trajectories to correct
* its motion and stick to the main trajectory.
**/
SoftMoveBaseAction(std::string name) :
as_(nh_, name, boost::bind(&SoftMoveBaseAction::executeCB, this, _1), false),
action_name_(name)
{
as_.start();
_dt = 0.1;
//_tmpTrajDt = 1.0;
_timeFromStart = 0;
_tmpTimeFromStart = 0;
_timer = 0;
_tolerance[0] = 0.01;
_tolerance[1] = 0.01;
_tolerance[2] = 0.5;
// last velocity and acceleration sent init
for(int i =0; i<6; ++i)
{
_lastVel[i] = 0.0;
_lastAcc[i] = 0.0;
}
_nextRobotCond_r.resize(6);
_newRobotCond_r.resize(6);
_currRobotCond_r.resize(6);
// initialize the limits, there are updated for
// each new received trajectory
for(int i=0; i<6; ++i)
{
limit.maxJerk = 0.9;
limit.maxAcc = 0.3;
limit.maxVel = 0.6;
limits.push_back(limit);
}
//wait for the listener to get the first message
_listener.waitForTransform("map", "base_footprint",
ros::Time(0), ros::Duration(1.0));
//set up the publisher for the cmd_vel topic
cmd_vel_pub = nh_.advertise<geometry_msgs::Twist>("base_controller/command", 1);
timeScaleSub_ = nh_.subscribe<std_msgs::Float64>("soft_move_base/timescale", 1, &SoftMoveBaseAction::timeScaleCB, this);
timeScale_ = 1;
ROS_INFO("soft_move_base action server up and ready");
}
TabletopGraspActionServer(std::string name) :
as_(nh_, name, boost::bind(&TabletopGraspActionServer::executeCB, this, _1), false),
action_name_(name)
{
heardPose = false;
heardJoinstState = false;
heardGrasps = false;
//create subscriber to joint angles
sub_angles = nh_.subscribe ("/joint_states", 1, &TabletopGraspActionServer::joint_state_cb, this);
//create subscriber to joint torques
sub_torques = nh_.subscribe ("/mico_arm_driver/out/joint_efforts", 1, &TabletopGraspActionServer::joint_effort_cb,this);
//create subscriber to tool position topic
sub_tool = nh_.subscribe("/mico_arm_driver/out/tool_position", 1, &TabletopGraspActionServer::toolpos_cb, this);
//subscriber for fingers
sub_finger = nh_.subscribe("/mico_arm_driver/out/finger_position", 1, &TabletopGraspActionServer::fingers_cb, this);
//subscriber for grasps
sub_grasps = nh_.subscribe("/find_grasps/grasps_handles",1, &TabletopGraspActionServer::grasps_cb,this);
//publish velocities
pub_velocity = nh_.advertise<geometry_msgs::TwistStamped>("/mico_arm_driver/in/cartesian_velocity", 10);
//publish pose array
pose_array_pub = nh_.advertise<geometry_msgs::PoseArray>("/agile_grasp_demo/pose_array", 10);
//publish pose
pose_pub = nh_.advertise<geometry_msgs::PoseStamped>("/agile_grasp_demo/pose_out", 10);
pose_fk_pub = nh_.advertise<geometry_msgs::PoseStamped>("/agile_grasp_demo/pose_fk_out", 10);
//debugging publisher
cloud_pub = nh_.advertise<sensor_msgs::PointCloud2>("agile_grasp_demo/cloud_debug", 10);
cloud_grasp_pub = nh_.advertise<sensor_msgs::PointCloud2>("agile_grasp_demo/cloud", 10);
ROS_INFO("Starting grasp action server...");
as_.start();
}
kr16_node_ros()
:as_follow_joint_trajectory_action_(n_, "follow_joint_trajectory_action", boost::bind(&kr16_node_impl::callback_follow_joint_trajectory_action_, &component_implementation_, _1, &as_follow_joint_trajectory_action_), false)
{
f = boost::bind(&kr16_node_ros::configure_callback, this, _1, _2);
server.setCallback(f);
as_follow_joint_trajectory_action_.start();
joint_states_ = n_.advertise<sensor_msgs::JointState>("joint_states", 1);
state_ = n_.advertise<control_msgs::JointTrajectoryControllerState>("state", 1);
n_.param("robot_ip_address", component_config_.robot_ip_address, (std::string)"192.1.10.20");
n_.param("robot_description", component_config_.robot_description, (std::string)"/home/ros/");
n_.param("robot_port", component_config_.robot_port, (int)49152);
}
spin_degreesAction(std::string name) :
// action server
as_(nh_, name, boost::bind(&spin_degreesAction::executeCB, this, _1), false),
action_name_(name)
{
// Subscribers and publishers
imu_sub_ = nh_.subscribe(IMU_SUBSCRIBER, 1, &spin_degreesAction::imu_callback, this);
cmd_vel_pub_=nh_.advertise<geometry_msgs::TwistStamped>(cmd_vel_PUBLISHER,1);
counter_=0;
success_=false;
as_.start();
}
PLAN2CTRLAction(std::string name) :
as_(nh_, name, boost::bind(&PLAN2CTRLAction::executeCB, this, _1), false),
action_name_(name)
{
ee_ft.resize(6);
// ROS TOPICS for controllers
sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(EE_STATE_POSE_TOPIC, 1, eeStateCallback);
sub_ft_ = nh_.subscribe<geometry_msgs::WrenchStamped>(EE_STATE_FT_TOPIC, 1, eeFTCallback);
pub_ = nh_.advertise<geometry_msgs::PoseStamped>(EE_CMD_POSE_TOPIC, 1);
pub_ft_ = nh_.advertise<geometry_msgs::WrenchStamped>(EE_CMD_FT_TOPIC, 1);
pub_gmr_out_ = nh_.advertise<std_msgs::Float32>("GMR_OUT", 1);
pub_action_state_ = nh_.advertise<std_msgs::String>("Action_State", 1000);
pub_action_name_ = nh_.advertise<std_msgs::String>("/motion_planner/action_name", 1000);
pub_model_fname_ = nh_.advertise<std_msgs::String>("/motion_planner/model_file_name", 1000);
pub_plot_dyn_ = nh_.advertise<std_msgs::Int32>("/motion_planner/plot_dynamics", 1000);
pub_ee_pos_att_ = nh_.advertise<geometry_msgs::PoseStamped>("/motion_planner/ee_pos_att", 1000);
pub_ee_ft_att_ = nh_.advertise<geometry_msgs::WrenchStamped>("/motion_planner/ee_ft_att", 1000);
//sub_plot_pub_ = nh_.subscribe<std_msgs::Int32>("/motion_planner/plot_published", 10, ppCallback);
as_.start();
}
BTAction(std::string name) :
as_(nh_, name, boost::bind(&BTAction::executeCB, this, _1), false),
action_name_(name)
{
as_.start();
robot_ip = "192.168.0.188";
std::cout << "Robot ip to use is: " << robot_ip << std::endl;
motion_proxy_ptr = new AL::ALMotionProxy("192.168.0.188", 9559);
//put this in init()
AL::ALValue stiffness_name("Body");
AL::ALValue stiffness(1.0f);
AL::ALValue stiffness_time(1.0f);
//motion_proxy_ptr->stiffnessInterpolation(stiffness_name,
// stiffness,
// stiffness_time);
//motion_proxy_ptr->moveInit();
}
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);
}
Node()
: private_nh("~")
, actionserver(nh, "moveto", false)
, disabled(false)
, kill_listener(nh, "kill")
, waypoint_validity_(nh)
{
// Make sure alarm integration is ok
kill_listener.waitForUpdate(ros::Duration(10));
if (kill_listener.getNumConnections() < 1)
throw std::runtime_error("The kill listener isn't connected to the alarm server");
kill_listener.start(); // F**k.
fixed_frame = mil_tools::getParam<std::string>(private_nh, "fixed_frame");
body_frame = mil_tools::getParam<std::string>(private_nh, "body_frame");
limits.vmin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
limits.vmax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
limits.amin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amin_b");
limits.amax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amax_b");
limits.arevoffset_b = mil_tools::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
limits.umax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "umax_b");
traj_dt = mil_tools::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));
waypoint_check_ = mil_tools::getParam<bool>(private_nh, "waypoint_check");
odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));
trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
trajectory_vis_pub = private_nh.advertise<PoseStamped>("trajectory_v", 1);
waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);
update_timer = nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));
actionserver.start();
set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
"set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
}
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;
}
FaceRecognition(std::string name) :
_as(_nh, name, boost::bind(&FaceRecognition::executeCB, this, _1), false),
_it(_nh),
_ph("~"),
_action_name(name),
_fd(),
_fr(NULL),
_fc(NULL),
_windowName("FaceRec"),
_window_rows(0),
_window_cols(0)
{
_as.start();
// Subscrive to input video feed and publish output video feed
// _image_sub = _it.subscribe("/camera/image_raw", 1, &FaceRecognition::imageCb, this);
// _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::imageCb, this);
// Get params
_ph.param<std::string>("algorithm", _recognitionAlgo , "lbph");
_ph.param<int>("no_training_images", _noTrainingImgs , 20);
_ph.param<int>("max_faces", _maxFaces , 2);
_ph.param<bool>("display_window", _displayWindow, true);
_ph.param<string>("preprocessing", _preprocessing, "tantriggs");
try {
_fr = new FaceRec(true, _preprocessing);
ROS_INFO("Using %s for recognition.", _recognitionAlgo.c_str());
ROS_INFO("Using %s for preprocessing.", _preprocessing.c_str());
}
catch( cv::Exception& e ) {
const char* err_msg = e.what();
ROS_INFO("%s", err_msg);
ROS_INFO("Only ADD_IMAGES mode will work. Please add more subjects for recognition.");
}
cv::namedWindow(_windowName);
}
GoToSelectedBall(std::string name) :
as_(nh_, name, boost::bind(&GoToSelectedBall::executeCB, this, _1), false),
action_name_(name),
ac("move_base", true)
{
selected_ball_sub_ = nh_.subscribe < geometry_msgs::Point > ("/one_selected_ball", 1, &GoToSelectedBall::selectedBallCb, this);
hoover_state_pub_ = nh_.advertise<std_msgs::Int16> ("hoover_state",1);
go_forward_robot_pub_ = nh_.advertise< std_msgs::Float32>("/robot_go_straight",1);
go_forward_robot_state_sub_ = nh_.subscribe("/robot_go_straight_state", 1, &GoToSelectedBall::robotGoStraightStateCb, this);
alghoritm_state_pub_ = nh_.advertise<std_msgs::String> ("/alghoritm_state",1);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &GoToSelectedBall::deadlockServiceStateCb, this);
firstGoalSent = false;
isBallPoseSet = false;
moveStraightState = 0;
moveStraightStateChange = false;
is_deadlock_service_run = false;
as_.start();
state_ = STOP;
}
WaitUntilUnblockedAction (std::string name) :
as_(nh_, name, boost::bind(&WaitUntilUnblockedAction::executeCB, this, _1), false),
action_name_(name)
{
// Point head
//Initialize the client for the Action interface to the head controller
point_head_client_ = new
PointHeadClient("/head_traj_controller/point_head_action", true);
//wait for head controller action server to come up
while(!point_head_client_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the point_head_action server to come up");
}
// human detection
activated = false;
human_detected = false;
obstacle_detected = false;
marker_pub = nh_.advertise<visualization_msgs::Marker>("obstacle_boundingbox", 1);
as_.start();
}
FibonacciAction(std::string name)
: as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1),
false),
action_name_(name) {
as_.start();
}