CoaxSimpleControl::CoaxSimpleControl(ros::NodeHandle &node)
:reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,coax_state_sub(node.subscribe("state",1, &CoaxSimpleControl::coaxStateCallback, this))
,coax_tf_sub(node.subscribe("tf",1, &CoaxSimpleControl::coaxTfCallback, this))
,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
,simple_control_pub(node.advertise<coax_msgs::CoaxControl>("simplecontrol",1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,mil_count(0)
,rotor_ready_count(-1)
,battery_voltage(12.22)
,imu_y(0.0),imu_r(0.0),imu_p(0.0)
,range_al(0.0)
,rc_th(0.0),rc_y(0.0),rc_r(0.0),rc_p(0.0)
,rc_trim_th(0.0),rc_trim_y(0.104),rc_trim_r(0.054),rc_trim_p(0.036)
,img_th(0.0),img_y(0.0),img_r(0.0),img_p(0.0)
,gyro_ch1(0.0),gyro_ch2(0.0),gyro_ch3(0.0)
,accel_x(0.0),accel_y(0.0),accel_z(0.0)
,mag_1(0.0), mag_2(0.0), mag_3(0.0)
,motor_up(0),motor_lo(0)
,servo_roll(0),servo_pitch(0)
,roll_trim(0),pitch_trim(0)
,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
,yaw_des(0.0),yaw_rate_des(0.0)
,roll_des(0.0),roll_rate_des(0.0)
,pitch_des(0.0),pitch_rate_des(0.0)
,altitude_des(1.5), coax_global_x(0.0), coax_global_y(0.0), coax_global_z(0.0)
,qnd0(0.0), qnd1(0.0), qnd2(0.0), qnd3(0.0)
,Gx_des(0.0), Gy_des(0.0)
,PI(3.14159265), r2d(57.2958), d2r(0.0175)
,ang_err_lim(0.14)
,x_vel_hist_1(0.0), x_vel_hist_2(0.0), x_vel_hist_3(0.0) , x_vel_hist_4(0.0)
,y_vel_hist_1(0.0), y_vel_hist_2(0.0), y_vel_hist_3(0.0) , y_vel_hist_4(0.0)
,x_gyro_1(0.0), x_gyro_2(0.0)
,y_gyro_1(0.0), y_gyro_2(0.0)
,Gz_old1(0.0), Gz_old2(0.0), Gz_old3(0.0)
,way_changed(0)
,way_old{0.0, 0.0, 0.0}
{
set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxSimpleControl::setNavMode, this));
set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxSimpleControl::setControlMode, this));
set_waypoint.push_back(node.advertiseService("set_waypoint", &CoaxSimpleControl::setWaypoint, this));
loadParams(node);
}
PeoplePositionServer()
{
rein_sub_
= n.subscribe("/humans/RecogInfo", 1,
&PeoplePositionServer::callback, this);
face_now_pub_
= n.advertise<humans_msgs::PersonPoseImgArray
>("/now_human", 10);
track_srv_
= n.advertiseService("track_srv",
&PeoplePositionServer::resTrackingId, this);
okao_srv_
= n.advertiseService("okao_srv",
&PeoplePositionServer::resOkaoId, this);
name_srv_
= n.advertiseService("name_srv",
&PeoplePositionServer::resName, this);
okaoStack_
= n.serviceClient<okao_client::OkaoStack>("stack_send");
//array_srv_
// = n.advertiseService("array_srv",
// &PeoplePositionServer::resHumans, this);
}
OnboardControl::OnboardControl(ros::NodeHandle &node)
:
reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,onboard_stage_pub(node.advertise<coax_msgs::stageState>("onboard_stage",1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_STATE(true)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,INIT_IMU(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,init_imu_count(0)
,last_state_time(0.0)
{
set_nav_mode = node.advertiseService("set_nav_mode", &OnboardControl::setNavMode, this);
set_control_mode = node.advertiseService("set_control_mode", &OnboardControl::setControlMode, this);
}
//! Initialize plugin - called in server constructor
void srs_env_model::CCMapPlugin::init(ros::NodeHandle & node_handle)
{
m_collisionMapVersion = 0;
m_dataBuffer->boxes.clear();
m_data->boxes.clear();
// Read parameters
// Get collision map radius
node_handle.param("collision_map_radius", m_collisionMapLimitRadius, COLLISION_MAP_RADIUS_LIMIT );
// Get collision map topic name
node_handle.param("collision_map_publisher", m_cmapPublisherName, COLLISION_MAP_PUBLISHER_NAME );
// Get FID to which will be points transformed when publishing collision map
node_handle.param("collision_map_frame_id", m_cmapFrameId, COLLISION_MAP_FRAME_ID ); //
// Create and publish service - get collision map
m_serviceGetCollisionMap = node_handle.advertiseService( GetCollisionMap_SRV, &CCMapPlugin::getCollisionMapSrvCallback, this);
// Create and publish service - is new collision map
m_serviceIsNewCMap = node_handle.advertiseService( IsNewCMap_SRV, &CCMapPlugin::isNewCmapSrvCallback, this );
// Create and publish service - lock map
m_serviceLockCMap = node_handle.advertiseService( LockCMap_SRV, &CCMapPlugin::lockCmapSrvCallback, this );
// Create and publish service - remove cube from map
m_serviceRemoveCube = node_handle.advertiseService( RemoveCubeCMP_SRV, &CCMapPlugin::removeBoxCallback, this );
// Create and publish service - add cube to map
m_serviceAddCube = node_handle.advertiseService( AddCubeCMP_SRV, &CCMapPlugin::addBoxCallback, this );
// Connect publishing services
pause( false, node_handle );
}
SnapshotMap( ros::NodeHandle& nh ) : nh_( nh ),object_available(false) {
imageAllocator_ = boost::shared_ptr< MultiResolutionSurfelMap::ImagePreAllocator >( new MultiResolutionSurfelMap::ImagePreAllocator() );
treeNodeAllocator_ = boost::shared_ptr< spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue > >( new spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue >( 10000 ) );
snapshot_service_ = nh.advertiseService("snapshot", &SnapshotMap::snapshotRequest, this);
object_pose_service_ = nh.advertiseService("object_pose", &SnapshotMap::poseRequest, this);
pub_cloud = pcl_ros::Publisher<pcl::PointXYZRGB>(nh, "output_cloud", 1);
pub_status_ = nh.advertise< std_msgs::Int32 >( "status", 1 );
tf_listener_ = boost::shared_ptr< tf::TransformListener >( new tf::TransformListener() );
//added to debug
m_debugObjPub = nh.advertise<geometry_msgs::PoseStamped>(
"debug_object", 1, true);
nh.param<double>( "max_resolution", max_resolution_, 0.0125 );
nh.param<double>( "max_radius", max_radius_, 30.0 );
nh.param<double>( "dist_dep_factor", dist_dep_, 0.005 );
nh.param<std::string>( "map_folder", map_folder_, "." );
nh.param<std::string>( "init_frame", init_frame_, "" );
create_map_ = false;
do_publish_tf_= true;
responseId_ = -1;
}
TaskScheduler::TaskScheduler(ros::NodeHandle & nh, boost::shared_ptr<TaskDefinition> tidle, double deftPeriod)
{
aqid = 0;
runScheduler = false;
idle = tidle;
idleTimeout = 0.001;
tasks["idle"] = idle;
defaultPeriod = deftPeriod;
startingTime = ros::Time::now().toSec();
mainThread.reset();
pthread_mutex_init(&scheduler_mutex,NULL);
pthread_cond_init(&scheduler_condition,NULL);
pthread_mutex_init(&aqMutex,NULL);
pthread_cond_init(&aqCond,NULL);
ROS_INFO("Task scheduler created: debug %d\n",debug);
n = nh;
startTaskSrv = nh.advertiseService("start_task", &TaskScheduler::startTask,this);
stopTaskSrv = nh.advertiseService("stop_task", &TaskScheduler::stopTask,this);
getTaskListSrv = nh.advertiseService("get_all_tasks", &TaskScheduler::getTaskList,this);
getTaskListLightSrv =nh.advertiseService("get_all_tasks_light", &TaskScheduler::getTaskListLight,this);
getAllTaskStatusSrv = nh.advertiseService("get_all_status", &TaskScheduler::getAllTaskStatus,this);
statusPub = nh.advertise<task_manager_msgs::TaskStatus>("status",20);
keepAliveSub = nh.subscribe("keep_alive",1,&TaskScheduler::keepAliveCallback,this);
lastKeepAlive = ros::Time::now();
}
// Constructor
NodeClass()
{
// initialization of variables
is_initialized_bool_ = false;
last_time_ = ros::Time::now();
x_rob_m_ = 0.0;
y_rob_m_ = 0.0;
theta_rob_rad_ = 0.0;
// set status of drive chain to WARN by default
drive_chain_diagnostic_ = diagnostic_status_lookup_.WARN;
// implementation of topics
// published topics
topic_pub_joint_state_cmd_ = n.advertise<sensor_msgs::JointState>("JointStateCmd", 1);
topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("Odometry", 50);
// subscribed topics
topic_sub_CMD_pltf_twist_ = n.subscribe("cmd_vel", 1, &NodeClass::topicCallbackTwistCmd, this);
topic_sub_EM_stop_state_ = n.subscribe("EMStopState", 1, &NodeClass::topicCallbackEMStop, this);
topic_sub_drive_diagnostic_ = n.subscribe("Diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
//<diagnostic_msgs::DiagnosticStatus>("Diagnostic", 1);
// implementation of service servers
srv_server_init_ = n.advertiseService("Init", &NodeClass::srvCallbackInit, this);
srv_server_reset_ = n.advertiseService("Reset", &NodeClass::srvCallbackReset, this);
srv_server_shutdown_ = n.advertiseService("Shutdown", &NodeClass::srvCallbackShutdown, this);
// implementation of service clients
srv_client_get_joint_state_ = n.serviceClient<cob_srvs::GetJointState>("GetJointState");
}
FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
as_(nh, nh.getNamespace(),
boost::bind(&FootstepPlanner::planCB, this, _1), false)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
srv_->setCallback (f);
pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
"text", 1, true);
pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"close_list", 1, true);
pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"open_list", 1, true);
srv_project_footprint_ = nh.advertiseService(
"project_footprint", &FootstepPlanner::projectFootPrintService, this);
srv_project_footprint_with_local_search_ = nh.advertiseService(
"project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
{
boost::mutex::scoped_lock lock(mutex_);
if (!readSuccessors(nh)) {
return;
}
JSK_ROS_INFO("building graph");
buildGraph();
JSK_ROS_INFO("build graph done");
}
sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
as_.start();
}
///Constructor
PowerCubeChainNode()
{
n_ = ros::NodeHandle("~");
pc_params_ = new PowerCubeCtrlParams();
pc_ctrl_ = new PowerCubeCtrl(pc_params_);
/// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState> ("/joint_states", 1);
topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("state", 1);
topicPub_OperationMode_ = n_.advertise<std_msgs::String> ("current_operationmode", 1);
topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
/// implementation of topics to subscribe
topicSub_CommandPos_ = n_.subscribe("command_pos", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
topicSub_CommandVel_ = n_.subscribe("command_vel", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);
/// implementation of service servers
srvServer_Init_ = n_.advertiseService("init", &PowerCubeChainNode::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("stop", &PowerCubeChainNode::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("recover", &PowerCubeChainNode::srvCallback_Recover, this);
srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);
initialized_ = false;
stopped_ = true;
error_ = false;
last_publish_time_ = ros::Time::now();
}
CloudMatcher::CloudMatcher(ros::NodeHandle& n):
n(n),
serviceMatchClouds(n.advertiseService("match_clouds", &CloudMatcher::match, this)),
serviceMatchCloudsWithGuess(n.advertiseService("match_clouds_with_guess", &CloudMatcher::matchWithGuess, this))
{
// load config
string configFileName;
if (ros::param::get("~config", configFileName))
{
ifstream ifs(configFileName.c_str());
if (ifs.good())
{
icp.loadFromYaml(ifs);
}
else
{
ROS_ERROR_STREAM("Cannot load config from YAML file " << configFileName);
icp.setDefault();
}
}
else
{
ROS_WARN_STREAM("No config file specified, using default ICP chain.");
icp.setDefault();
}
// replace logger
if (getParam<bool>("useROSLogger", false))
PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
}
EventsGenerator::EventsGenerator(ros::NodeHandle& n, ros::NodeHandle& np)
{
double p;
np.param("exp_timeout", p, 0.5);
exp_timeout_ = ros::Duration(p);
sub_desires_ = n.subscribe("desires_set", 1,
&EventsGenerator::desiresCB, this);
sub_intention_ = n.subscribe("intention", 1,
&EventsGenerator::intentionCB, this);
srv_cem_ = n.advertiseService("create_exploitation_matcher",
&EventsGenerator::cemCB, this);
srv_ctem_ = n.advertiseService("create_topic_exploitation_matcher",
&EventsGenerator::ctemCB, this);
pub_events_ = n.advertise<hbba_msgs::Event>("events", 100);
ros::NodeHandle rosgraph_np(np, "rosgraph_monitor");
rosgraph_monitor_.reset(new RosgraphMonitor(n, rosgraph_np));
rosgraph_monitor_->registerCB(&EventsGenerator::rosgraphEventsCB, this);
timer_ = n.createTimer(ros::Duration(1.0), &EventsGenerator::timerCB, this);
parseExploitationMatches(n);
}
CaptureServer() :
nh_private("~") {
ROS_INFO("Creating localization");
queue_size_ = 5;
odom_pub = nh_.advertise<nav_msgs::Odometry>("vo", queue_size_);
keyframe_pub = nh_.advertise<rm_localization::Keyframe>("keyframe",
queue_size_);
update_map_service = nh_.advertiseService("update_map",
&CaptureServer::update_map, this);
send_all_keyframes_service = nh_.advertiseService("send_all_keyframes",
&CaptureServer::send_all_keyframes, this);
clear_keyframes_service = nh_.advertiseService("clear_keyframes",
&CaptureServer::clear_keyframes, this);
rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);
// Synchronize inputs.
sync.reset(
new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
info_sub));
sync->registerCallback(
boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));
}
ShooterVision() : nh_(ros::this_node::getName()), it_(nh_) {
nh_.param<bool>("auto_start", active, false);
vision.reset(new GrayscaleContour(nh_));
vision->init();
nh_.param<std::string>("symbol_camera", camera_topic,
"/right/right/image_raw");
std::string get_shapes_topic;
nh_.param<std::string>("get_shapes_topic",get_shapes_topic,"get_shapes");
runService = nh_.advertiseService(get_shapes_topic+"/switch", &ShooterVision::runCallback, this);
roiService = nh_.advertiseService("setROI",
&ShooterVision::roiServiceCallback, this);
//#ifdef DO_DEBUG
// DebugWindow::init();
//#endif
foundShapesPublisher = nh_.advertise<navigator_msgs::DockShapes>(
"found_shapes", 1000);
image_sub_ = it_.subscribe(camera_topic, 1, &ShooterVision::run, this);
int x_offset, y_offset, width, height;
nh_.param<int>("roi/x_offset", x_offset, 73);
nh_.param<int>("roi/y_offset", y_offset, 103);
nh_.param<int>("roi/width", width, 499);
nh_.param<int>("roi/height", height, 243);
nh_.param<int>("size/height", size.height, 243);
nh_.param<int>("size/width", size.width, 243);
roi = Rect(x_offset, y_offset, width, height);
TrackedShape::init(nh_);
tracker.init(nh_);
}
bool ExcavaROBArmKinematics::init() {
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
nh_.param("urdf_xml", urdf_xml, std::string("robot_description"));
nh_.searchParam(urdf_xml, full_urdf_xml);
ROS_DEBUG("ExcavaROB Kinematics: Reading xml file from parameter server");
std::string result;
if (!nh_.getParam(full_urdf_xml, result)) {
ROS_FATAL("ExcavaROB Kinematics: Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root, Wrist and Tip From Parameter Service
if (!nh_private_.getParam("root_name", root_name_)) {
ROS_FATAL("ExcavaROB Kinematics: No root_name found on parameter server");
return false;
}
if (!nh_private_.getParam("tip_name", tip_name_)) {
ROS_FATAL("ExcavaROB Kinematics: No tip_name found on parameter server");
return false;
}
if (!nh_private_.getParam("max_iterations", max_iterations_)) {
ROS_FATAL("ExcavaROB Kinematics: No max_iterations found on parameter server");
return false;
}
if (!nh_private_.getParam("eps", eps_)) {
ROS_FATAL("ExcavaROB Kinematics: No eps found on parameter server");
return false;
}
if (!nh_private_.getParam("ik_gain", ik_gain_)) {
ROS_FATAL("ExcavaROB Kinematics: No ik_gain found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int maxIterations;
double epsilon;
nh_private_.param("maxIterations", maxIterations, 1000);
nh_private_.param("epsilon", epsilon, 1e-2);
// Build Solvers
fk_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
jnt_to_jac_solver_ = new KDL::ChainJntToJacSolver(arm_chain_);
jnt_to_pose_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
ROS_INFO("ExcavaROB Kinematics: Advertising services");
ik_service_ = nh_private_.advertiseService(IK_SERVICE, &ExcavaROBArmKinematics::getPositionIK, this);
fk_service_ = nh_private_.advertiseService(FK_SERVICE, &ExcavaROBArmKinematics::getPositionFK, this);
kin_solver_info_service_ = nh_private_.advertiseService(KIN_INFO_SERVICE, &ExcavaROBArmKinematics::getKinSolverInfo, this);
return true;
}
void init()
{
nh.initNode();
//~ nh.subscribe(sub);
nh.advertiseService(fireService);
nh.advertiseService(cancelService);
nh.advertiseService(manualService);
}
T11::T11(ros::NodeHandle node) {
knowledge_pub = node.advertise<t11_kb_modeling::Knowledge>(TOPIC_KB, 100);
position_sub = node.subscribe(TOPIC_ROBOT_LOCATION, 100, &T11::positionCallback, this);
hri_feature_sub = node.subscribe("t31_feature", 10, &T11::hriFeatureCallback, this);
env_feature_sub = node.subscribe("t21_feature", 10, &T11::envFeatureCallback, this);
service_get_location = node.advertiseService(SERVICE_GET_LOCATION, &T11::getLocation, this);
service_get_all_sites = node.advertiseService(SERVICE_GET_ALL_SITES, &T11::getAllSites, this);
}
bool Kinematics::init() {
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
nh.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!nh.getParam(full_urdf_xml, result)) {
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
std::string kinematics_service_name;
if (!nh_private.getParam("kinematics_service_name", kinematics_service_name)) {
ROS_FATAL("GenericIK: kinematics service name not found on parameter server");
return false;
}
// Get Root and Tip From Parameter Service
if (!nh.getParam(kinematics_service_name+"/manipulator/root_name", root_name)) {
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!nh.getParam(kinematics_service_name+"/manipulator/tip_name", tip_name)) {
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int maxIterations;
double epsilon;
nh_private.param("maxIterations", maxIterations, 1000);
nh_private.param("epsilon", epsilon, 1e-2); // .01
// Build Solvers
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, maxIterations, epsilon);
ROS_INFO("Advertising services");
fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
// ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::searchPositionIK,this);
ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
return true;
}
Action_client_cmd_interface::Action_client_cmd_interface(ros::NodeHandle &nh,
ac::Kuka_action_client &kuka_action_client,
const std::string &action_service_name,
const std::string &cmd_service_name)
:
nh(nh),
kuka_action_client(kuka_action_client),
curr_action_state(action_states::PENDING)
{
action_service = nh.advertiseService(action_service_name,&Action_client_cmd_interface::action_service_callback,this);
cmd_interface_service = nh.advertiseService(cmd_service_name,&Action_client_cmd_interface::cmd_interface_callback,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;
}
void BaseStation::initHandlers(ros::NodeHandle & node) {
getObstaclesPositionService = node.advertiseService("basestation/getObstaclesPosition", &BaseStation::getObstaclesPosition, this);
findRobotPositionAndAngleService = node.advertiseService("basestation/findRobotPositionAndAngle", &BaseStation::findRobotPositionAndAngle, this);
showSolvedSudocubeService = node.advertiseService("basestation/showSolvedSudocube", &BaseStation::showSolvedSudocube, this);
traceRealTrajectoryService = node.advertiseService("basestation/traceRealTrajectory", &BaseStation::traceRealTrajectory, this);
loopEndedService = node.advertiseService("basestation/loopEnded", &BaseStation::loopEnded, this);
startLoopClient = node.serviceClient<kinocto::StartLoop>("kinocto/startLoop");
setRobotPositionAndAngleClient = node.serviceClient<kinocto::SetRobotPositionAndAngle>("kinocto/setRobotPositionAndAngle");
updateRobotSubscriber = node.subscribe("basestation/updateRobotPosition", 50, &BaseStation::updateRobotPosition, this);
}
bool ForceTorqueNode::init()
{
m_isInitialized = false;
//topicPub_ForceData_ = nh_.advertise<std_msgs::Float32MultiArray>("/arm_controller/wrench", 100);
topicPub_ForceData_ = nh_.advertise<geometry_msgs::WrenchStamped>("/arm_controller/wrench", 100);
//topicPub_ForceDataBase_ = nh_.advertise<std_msgs::Float32MultiArray>("/arm_controller/wrench_bl", 100);
topicPub_Marker_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
srvServer_Init_ = nh_.advertiseService("/ft_controller/init", &ForceTorqueNode::srvCallback_Init, this);
srvServer_Calibrate_ = nh_.advertiseService("/ft_controller/calibrate", &ForceTorqueNode::srvCallback_Calibrate, this);
return true;
}
CoaxVisionControl::CoaxVisionControl(ros::NodeHandle &node)
:VisionFeedback(node), KF(node)
,reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,vicon_state_sub(node.subscribe("coax_57/vicon/odom",1,&CoaxVisionControl::viconCallback,this))
,coax_state_sub(node.subscribe("state",1, &CoaxVisionControl::StateCallback,this))
,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
,vision_control_pub(node.advertise<coax_msgs::CoaxControl>("visioncontrol",1))
,vicon_control_pub(node.advertise<coax_msgs::viconControl>("vicon_control",1))
,vicon_state_pub(node.advertise<coax_msgs::viconState>("vicon_state",1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_STATE(true)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,INIT_IMU(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,init_imu_count(0)
,rotor_ready_count(-1)
,last_state_time(0.0)
,rpy(0,0,0), accel(0,0,0), gyro(0,0,0), rpyt_rc(0,0,0,0), rpyt_rc_trim(0,0,0,0)
,range_al(0.0)
,pos_z(0.0),vel_z(0.0)
,motor_up(0),motor_lo(0)
,servo_roll(0),servo_pitch(0)
,roll_trim(0),pitch_trim(0)
,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
,des_yaw(0.0),yaw_rate_des(0.0)
,roll_des(0.0),roll_rate_des(0.0)
,pitch_des(0.0),pitch_rate_des(0.0)
,altitude_des(0.0)
,stage(0),initial_pos(0),des_pos_x(0.0),des_pos_y(0.0),des_pos_z(0.0)
,des_pos_x_origin(0.0),des_pos_y_origin(0.0)
{
// vicon_state_sub=node.subscribe("coax_57/vicon/odom",1,&CoaxVisionControl::viconCallback,this);
set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxVisionControl::setNavMode, this));
set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxVisionControl::setControlMode, this));
}
Peg_replay::Peg_replay(ros::NodeHandle& node,
const std::string& plug_topic,
const std::string& socket_topic):
broadcaster_plug_link("world",plug_topic),broadcaster_socket("world",socket_topic),
index_subscriber(node,"index_sub")
{
bPlayTraj = false;
bInit = false;
bListen_index = false;
service = node.advertiseService("peg_replay_cmd", &Peg_replay::cmd_callback,this);
ptr_node = &node;
cmds["load"] = LOAD;
cmds["start"] = START;
cmds["stop"] = STOP;
cmds["init"] = INIT;
cmds["listen"] = LIST_INDEX;
position_socket.setValue(0,0,0);
plug_link_pos.setValue(0.03,-0.04,0);
plug_link_rot.setRPY(0,0,M_PI);
run_trajectories.set_initial_position(plug_link_pos,plug_link_rot);
t = 0;
t_stop = 0;
}
SceneSegmentation()
: nh_("~"), it_(nh_)
{
nh_.param<float>("sigma", sigma_, 0.5f);
nh_.param<float>("k", k_, 500.f);
nh_.param<int>("min", min_, 20);
nh_.param<bool>("half", half_, true);
bool draw_rect;
nh_.param<bool>("draw_rect", draw_rect, true);
ROS_INFO("Waiting for message on camera info topic.");
camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh_);
// segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, half_));
if(half_)
segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width/2, camera_info_->height/2, draw_rect));
else
segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, draw_rect));
srv_segment_ = nh_.advertiseService("segment_image", &SceneSegmentation::segment_image_cb, this);
ROS_INFO("segment_image is ready.");
sub_image_ = it_.subscribe("image_in", 1, &SceneSegmentation::image_cb, this);
pub_image_ = it_.advertise("image_out", 1);
pub_rects_ = nh_.advertise<duckietown_msgs::Rects>("rects_out", 1);
pub_segments_ = nh_.advertise<duckietown_msgs::SceneSegments>("segments_out", 1);
}
MyClass()
: p_nh("~")
{
sub = nh.subscribe("/humans/kinect_v2", 1, &MyClass::callback, this);
recog_sub = nh.subscribe("/humans/recog_info", 1, &MyClass::recogCallback, this);
dispub = nh.advertise<std_msgs::Float64>("/displacement", 1);
srv = nh.advertiseService("displace", &MyClass::Service, this);
pt.x = 5.0;
pt.y = 0;
pt.z = 0;
cv::namedWindow(OPENCV_WINDOW);
index = 0;
attention_t_id = 0;
srv_switch = false;
p_nh.param("width", width, 1000);
p_nh.param("height", height, 500);
o_id = 0;
prev_time = ros::Time::now();
now_time = ros::Time::now();
count_time = 0;
}
JointsClient()
{
//ros::Rate loop(10);
srv = n.serviceClient<humans_msgs::HumanImgSrv>("okao_srv");
viz_pub
= n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
pps_pub
= n.advertise<humans_msgs::PersonPoseImgArray>("/v_human", 10);
joints_last
= n.advertiseService("joints_last_srv",
&JointsClient::jointsLastRequest, this);
/*
okao_id.push_back(1);
okao_id.push_back(2);
okao_id.push_back(3);
okao_id.push_back(4);
okao_id.push_back(5);
okao_id.push_back(6);
okao_id.push_back(7);
okao_id.push_back(8);
okao_id.push_back(9);
okao_id.push_back(10);
okao_id.push_back(11);
okao_id.push_back(12);
okao_id.push_back(13);
*/
//joints_stream
// = n.advertiseService("joints_stream_srv",
// &JointsClient::JointsStreamRequest, this);
//okaoStack
// = n.serviceClient<okao_client::OkaoStack>("stack_send");
}
IKTrajectoryExecutor()
{
//tell the joint trajectory action client that we want
//to spin a thread by default
action_client = new TrajClient("joint_trajectory_action", true);
//wait for the action server to come up
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
//register a service to input desired Cartesian trajectories
service = node.advertiseService("execute_cartesian_ik_trajectory",
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
//have to specify the order of the joints we're sending in our
//joint trajectory goal, even if they're already on the param server
goal.trajectory.joint_names.push_back("shoulder_pan_joint");
goal.trajectory.joint_names.push_back("shoulder_lift_joint");
goal.trajectory.joint_names.push_back("elbow_joint");
goal.trajectory.joint_names.push_back("wrist_1_joint");
goal.trajectory.joint_names.push_back("wrist_2_joint");
goal.trajectory.joint_names.push_back("wrist_3_joint");
}
Sensor_manager::Sensor_manager(ros::NodeHandle& nh, wobj::WrapObject& wrapped_objects, obj::Socket_one &socket_one):
wrapped_objects(wrapped_objects),socket_one(socket_one)
{
t_sensor = psm::NONE;
service_server = nh.advertiseService("sensor_manager_cmd",&Sensor_manager::sensor_manager_callback,this);
initialise();
}
ObjectRecognition::ObjectRecognition(ros::NodeHandle node_handle) :
node_handle(node_handle),
point_cloud_subscriber(node_handle, "/camera/depth_registered/points", 1),
camera_info_subscriber(node_handle, "/camera/rgb/camera_info", 1),
rgbd_image_synchronizer(RgbdImagePolicy(10), point_cloud_subscriber, camera_info_subscriber)
{
boost::shared_ptr<std::vector<Object>> trained_objects(new std::vector<Object>);
this->loadModels(trained_objects);
this->object_pipeline = boost::shared_ptr<ObjectPipeline>(new ObjectPipeline(trained_objects));
this->objects_publisher = node_handle.advertise<sepanta_msgs::Objects>("/object_recognition/objects", 5);
this->turnOnService = node_handle.advertiseService("/object_recognition/turn_on", &ObjectRecognition::turnOn, this);
this->turnOffService = node_handle.advertiseService("/object_recognition/turn_off", &ObjectRecognition::turnOff, this);
}
CoaxInterface(ros::NodeHandle & n)
{
reach_nav_state = n.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state");
configure_comm = n.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm");
set_timeout = n.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout");
raw_control_pub = n.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1);
coax_info_pub = n.advertise<geometry_msgs::Quaternion>("info",1);
control_mode_pub = n.advertise<geometry_msgs::Quaternion>("control_mode",1);
coax_state_sub = n.subscribe("state", 10, &CoaxInterface::coaxStateCallback, this);
matlab_trim_sub = n.subscribe("trim", 10, &CoaxInterface::matlabTrimCallback, this);
matlab_nav_mode_sub = n.subscribe("nav_mode", 10, &CoaxInterface::matlabNavModeCallback, this);
matlab_raw_control_sub = n.subscribe("raw_control", 10, &CoaxInterface::matlabRawControlCallback, this);
set_control_mode.push_back(n.advertiseService("set_control_mode", &CoaxInterface::setControlMode, this));
roll_trim = 0;
pitch_trim = 0;
motor1 = 0;
motor2 = 0;
servo1 = 0;
servo2 = 0;
matlab_rawcontrol_age = 0;
coax_state_age = 0;
desired_nav_mode = 0;
}