ShapeGraspPlanner::ShapeGraspPlanner(ros::NodeHandle& nh)
{
/*
* Gripper model is based on having two fingers, and assumes
* that the robot is using the moveit_simple_controller_manager
* gripper interface, with "parallel" parameter set to true.
*/
nh.param<std::string>("gripper/left_joint", left_joint_, "l_gripper_finger_joint");
nh.param<std::string>("gripper/right_joint", right_joint_, "r_gripper_finger_joint");
nh.param("gripper/max_opening", max_opening_, 0.110);
nh.param("gripper/max_effort", max_effort_, 50.0);
nh.param("gripper/finger_depth", finger_depth_, 0.02);
nh.param("gripper/grasp_duration", grasp_duration_, 2.0);
nh.param("gripper/gripper_tolerance", gripper_tolerance_, 0.02);
/*
* Approach is usually aligned with wrist_roll
*/
nh.param<std::string>("gripper/approach/frame", approach_frame_, "wrist_roll_link");
nh.param("gripper/approach/min", approach_min_translation_, 0.145);
nh.param("gripper/approach/desired", approach_desired_translation_, 0.15);
/*
* Retreat is usually aligned with wrist_roll
*/
nh.param<std::string>("gripper/retreat/frame", retreat_frame_, "wrist_roll_link");
nh.param("gripper/retreat/min", retreat_min_translation_, 0.145);
nh.param("gripper/retreat/desired", retreat_desired_translation_, 0.15);
// Distance from tool point to planning frame
nh.param("gripper/tool_to_planning_frame", tool_offset_, 0.165);
}
/**
* Initialize parameters from launch file
* @param node a ROS node handle
*/
void Circular::initializeParameters(ros::NodeHandle &node){
Descriptor base, range;
// Loading parameters from a launch file
node.param("threshold", this->threshold_, 100);
node.param("min_score", this->blob_.min_score, 7);
node.getParam("landmark_diagonal", this->landmark_diag_);
// Base descriptor parameters
node.getParam("base_shape", base.invariant.shape);
node.getParam("base_area", base.invariant.area);
node.getParam("base_hu", base.invariant.hu);
// (Similarity) range descriptor parameters
node.getParam("range_shape", range.invariant.shape);
node.getParam("range_area", range.invariant.area);
node.getParam("range_hu", range.invariant.hu);
// Rotation of camera in respect to imu
double roll_deg, pitch_deg, yaw_deg;
node.param("rot_x", roll_deg, 0.0);
node.param("rot_y", pitch_deg, 0.0);
node.param("rot_z", yaw_deg, 0.0);
this->imuToCamRot_.setRPY(roll_deg*M_PI/180, pitch_deg*M_PI/180, yaw_deg*M_PI/180);
this->blob_.setBase(base);
this->blob_.setRange(range);
}
void initialize(UAS &uas_,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
double linear_stdev, angular_stdev, orientation_stdev, mag_stdev;
uas = &uas_;
nh.param<std::string>("imu/frame_id", frame_id, "fcu");
nh.param("imu/linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec
nh.param("imu/angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec
nh.param("imu/orientation_stdev", orientation_stdev, 1.0);
nh.param("imu/magnetic_stdev", mag_stdev, 0.0);
setup_covariance(linear_acceleration_cov, linear_stdev);
setup_covariance(angular_velocity_cov, angular_stdev);
setup_covariance(orientation_cov, orientation_stdev);
setup_covariance(magnetic_cov, mag_stdev);
setup_covariance(unk_orientation_cov, 0.0);
imu_pub = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
magn_pub = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10);
temp_pub = nh.advertise<sensor_msgs::Temperature>("imu/temperature", 10);
press_pub = nh.advertise<sensor_msgs::FluidPressure>("imu/atm_pressure", 10);
imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 10);
}
void readParameters(ros::NodeHandle pnh){
pnh.param("save_map_launch_package", save_map_launch_package , string("mapping_controller"));
pnh.param("save_map_launch_file", save_map_launch_file , string("save_map"));
pnh.param("map_file_path", map_file_path , string("/home/pi"));
pnh.param("robot_position_topic", robot_position_topic, string("/agent1/amcl_pose"));
pnh.param("robot_initial_position_topic", robot_initial_position_topic, string("/agent1/initialpose"));
}
bool JacoManipulation::loadParameters(const ros::NodeHandle n)
{
ROS_DEBUG("Loading parameters");
n.param("wpi_jaco/arm_name", arm_name_, std::string("jaco"));
n.param("wpi_jaco/gripper_closed", gripper_closed_, 0.0);
n.param("wpi_jaco/gripper_open", gripper_open_, 65.0);
n.param("wpi_jaco/num_fingers", num_fingers_, 3);
// Update topic prefix
if (arm_name_ == "jaco2")
topic_prefix_ = "jaco";
else
topic_prefix_ = arm_name_;
if (kinova_gripper_)
num_joints_ = num_fingers_ + NUM_JACO_JOINTS;
else
num_joints_ = NUM_JACO_JOINTS;
joint_pos_.resize(num_joints_);
ROS_INFO("arm_name: %s", arm_name_.c_str());
ROS_INFO("Parameters loaded.");
//! @todo MdL [IMPR]: Return is values are all correctly loaded.
return true;
}
CameraImpl(ros::NodeHandle& nh, ros::NodeHandle& nh_private, const openni::DeviceInfo& device_info) :
rgb_sensor_(new SensorStreamManagerBase()),
depth_sensor_(new SensorStreamManagerBase()),
ir_sensor_(new SensorStreamManagerBase()),
reconfigure_server_(nh_private)
{
device_.open(device_info.getUri());
printDeviceInfo();
printVideoModes();
buildResolutionMap();
device_.setDepthColorSyncEnabled(true);
std::string rgb_frame_id, depth_frame_id;
nh_private.param(std::string("rgb_frame_id"), rgb_frame_id, std::string("camera_rgb_optical_frame"));
nh_private.param(std::string("depth_frame_id"), depth_frame_id, std::string("camera_depth_optical_frame"));
if(device_.hasSensor(SENSOR_COLOR))
{
rgb_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_COLOR, "rgb", rgb_frame_id, resolutions_[Camera_RGB_640x480_30Hz]));
}
if(device_.hasSensor(SENSOR_DEPTH))
{
depth_sensor_.reset(new DepthSensorStreamManager(nh, device_, rgb_frame_id, depth_frame_id, resolutions_[Camera_DEPTH_640x480_30Hz]));
}
if(device_.hasSensor(SENSOR_IR))
{
ir_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_IR, "ir", depth_frame_id, resolutions_[Camera_IR_640x480_30Hz]));
}
reconfigure_server_.setCallback(boost::bind(&CameraImpl::configure, this, _1, _2));
}
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;
}
HDPCDrive(ros::NodeHandle & nh) : geom(nh) {
command_pub = nh.advertise<hdpc_com::Commands>("/hdpc_com/commands",1);
reading_sub = nh.subscribe("/hdpc_com/readings",1,&HDPCDrive::readingsCallback,this);
state_machine_client = nh.serviceClient<hdpc_com::ChangeStateMachine>("/hdpc_com/changeState");
control_mode_serv = nh.advertiseService("set_control_mode",&HDPCDrive::set_mode,this);
direct_command_sub = nh.subscribe("direct",1,&HDPCDrive::directDriveCallback,this);
ackermann_command_sub = nh.subscribe("ackermann",1,&HDPCDrive::ackermannCallback,this);
status_pub = nh.advertise<Status>("status",1);
// TODO: change default value to something that makes sense
nh.param("max_rotation_speed_rad_per_s",max_rotation_speed_rad_per_s,1.0);
nh.param("max_linear_speed_m_per_s",max_linear_speed_m_per_s,0.9);
nh.param("synchronise_steering",synchronise_steering,false);
nh.param("zero_on_init",zero_on_init,false);
watchdog = 0;
desired_elevation = M_PI/2;
control_mode = HDPCModes::MODE_INIT;
commands.header.stamp = ros::Time::now();
for (unsigned int i=0;i<10;i++) {
commands.isActive[i] = false;
commands.velocity[i] = 0;
commands.position[i] = 0.0;
}
}
CylinderDetection() :
m_NodeHandle("~")
{
m_NodeHandle.param("base_frame", m_strBaseFrame, std::string("/body"));
m_NodeHandle.param("max_range", m_dMaxRange, 5.0);
m_NodeHandle.param("n_samples", m_nSamples, 1000);
m_NodeHandle.param("ransac_tolerance", m_dRansacTolerance, 1.0);
m_NodeHandle.param("parse_plane_lower_bound_threshhold", THRESHOLD_LOWER_TO_PARSE_PLANE, 10);
m_NodeHandle.param("parse_plane_upper_bound_threshhold", THRESHOLD_UPPER_TO_PARSE_PLANE, 600);
m_NodeHandle.param("accept_circle_threshhold", THRESHOLD_TO_ACCEPT_CIRCLE_FOUND, 10);
m_NodeHandle.param("discretization_precision", m_dDiscretizationPrecision, 0.025);
m_NodeHandle.param("min_num_valid_sections_to_accept_cylinder", MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER, 4);
m_NodeHandle.param("tolerane_for_plane_concatenation", TOLERANCE_FOR_PLANE_CONCATENATION, 0.01);
m_NodeHandle.param("maximum_radius", MAXIMUM_RADIUS, 1.0);
ROS_INFO("Searching for vertical cylinders");
ROS_INFO("RANSAC: %d iteration with %f tolerance", m_nSamples, m_dRansacTolerance);
assert(m_nSamples > 0);
// Make sure TF is ready
ros::Duration(0.5).sleep();
m_Subscriber = m_NodeHandle.subscribe("/vrep/depthSensor", 1, &CylinderDetection::pc_callback, this);
// Initialize the random seed for RANSAC
srand(time(NULL));
}
FollowerFSM() : hold(true),
currentState(STOPPED),
nextState(STOPPED),
nPriv("~")
{
//hold = true;
//currentState = STOPPED;
//nextState = STOPPED;
nPriv.param<std::string>("world_frame", world_frame, "world_lol");
nPriv.param<std::string>("robot_frame", robot_frame, "robot");
nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
nPriv.param<double>("minimum_distance", minimum_distance, 2);
nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
nPriv.param("distance_to_target", distance_to_target, 1.5);
nPriv.param<double>("kv", kv, 0.03);
nPriv.param<double>("kalfa", kalfa, 0.08);
//nPriv.param<double>("rate", frequency, 10);
rate = new ros::Rate(10.0);
nPriv.param<std::string>("control_type", cType, "planner");
if(cType == "planner")
{ ac = new MoveBaseClient("move_base", true);}
notReceivedNumber = 0;
sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);
timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
timer.start();
}
void load_params(ros::NodeHandle &nh) {
string str_calib;
stringstream ss_calib;
nh.param("D", str_calib, string(""));
restoreVec(str_calib, cam_info.D);
nh.param("K", str_calib, string(""));
(str_calib, cam_info.K);
nh.param("R", str_calib, string(""));
restoreVec(str_calib, cam_info.R);
nh.param("P", str_calib, string(""));
restoreVec(str_calib, cam_info.P);
std::string frame_id;
nh.param<std::string>("frame_id",frame_id,"/camera");// /viz/uav_cam_front/camera
cam_info.header.frame_id = frame_id;
cout
<< "Current cam_info:"
<< endl;
cout << " <param name=\"D\" type=\"string\" value=\"" << arrayToString(
cam_info.D) << "\"/>" << endl;
cout << " <param name=\"K\" type=\"string\" value=\"" << arrayToString(
cam_info.K) << "\"/>" << endl;
cout << " <param name=\"R\" type=\"string\" value=\"" << arrayToString(
cam_info.R) << "\"/>" << endl;
cout << " <param name=\"P\" type=\"string\" value=\"" << arrayToString(
cam_info.P) << "\"/>" << endl;
}
int NodeClass::init()
{
if (n.hasParam("ComPort"))
{
n.getParam("ComPort", sComPort);
ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
}
else
{
sComPort ="/dev/ttyUSB0";
ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
}
n.param("message_timeout", relayboard_timeout_, 2.0);
n.param("protocol_version", protocol_version_, 1);
m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());
m_SerRelayBoard->init();
// Init member variable for EM State
EM_stop_status_ = ST_EM_ACTIVE;
duration_for_EM_free_ = ros::Duration(1);
return 0;
}
WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) :
nh_(nh), image_transport_(nh),
handler_group_(http_server::HttpReply::stock_reply(http_server::HttpReply::not_found))
{
cleanup_timer_ = nh.createTimer(ros::Duration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this));
int port;
private_nh.param("port", port, 8080);
int server_threads;
private_nh.param("server_threads", server_threads, 1);
private_nh.param("ros_threads", ros_threads_, 2);
stream_types_["mjpeg"] = boost::shared_ptr<ImageStreamerType>(new MjpegStreamerType());
stream_types_["vp8"] = boost::shared_ptr<ImageStreamerType>(new LibavStreamerType("webm", "libvpx", "video/webm"));
handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2));
handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2));
handler_group_.addHandlerForPath("/stream_viewer", boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2));
handler_group_.addHandlerForPath("/snapshot", boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2));
server_.reset(new http_server::HttpServer("0.0.0.0", boost::lexical_cast<std::string>(port),
boost::bind(ros_connection_logger, handler_group_, _1, _2), server_threads));
}
//! 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 );
}
void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
if(_use_dynamic_reconfig)
{
// classical PID init
_pid.init(_node);
}
else
{
control_toolbox::Pid::Gains gains;
// Load PID gains from parameter server
if (!_node.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", _node.getNamespace().c_str());
return;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
_node.param("i", gains.i_gain_, 0.0);
_node.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
_node.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
_pid.setGains(gains);
}
}
void PlaDyPosNode_v3::configure(ros::NodeHandle& nh, ros::NodeHandle& ph)
{
//Initialize subscribers and publishers
tau = nh.subscribe("tauIn", 1, &PlaDyPosNode_v3::onTau,this);
batteryVoltage = nh.subscribe("battery_voltage", 1, &PlaDyPosNode_v3::onBatteryVoltage,this);
tauAch = nh.advertise<auv_msgs::BodyForceReq>("tauAch",1);
revs = nh.advertise<std_msgs::Int16MultiArray>("pwm_out",1);
//Initialize max/min tau
double maxThrust(1),minThrust(-1);
//ph.param("maxThrust",maxThrust,maxThrust);
//ph.param("minThrust",minThrust,minThrust);
ph.param("max_cap",maxCap,maxCap);
ph.param("useWeighted",useWeighted, useWeighted);
//Initialize the allocation matrix
float cp(cos(M_PI/4)),sp(sin(M_PI/4));
B<<cp,cp,-cp,-cp,
-sp,sp,-sp,sp,
-1,1,1,-1;
BinvIdeal = B.transpose()*(B*B.transpose()).inverse();
//Scaling allocation only for XYN
allocator.configure(B,maxThrust,minThrust);
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&FaceDetectionNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FaceDetectionNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&FaceDetectionNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FaceDetectionNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::FaceArrayStamped>("faces", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
std::string face_cascade_name, eyes_cascade_name;
local_nh_.param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
local_nh_.param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };
dynamic_reconfigure::Server<face_detection::FaceDetectionConfig>::CallbackType f =
boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
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 srs_env_model::CCollisionObjectPlugin::init(ros::NodeHandle & node_handle)
{
node_handle.param("collision_object_publisher", m_coPublisherName, COLLISION_OBJECT_PUBLISHER_NAME );
node_handle.param("collision_object_frame_id", m_coFrameId, COLLISION_OBJECT_FRAME_ID );
// Create publisher
m_coPublisher = node_handle.advertise<arm_navigation_msgs::CollisionObject> (m_coPublisherName, 100, m_latchedTopics);
}
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;
}
void
fillCameraInfoMessage (ros::NodeHandle nh, std::string base_name, sensor_msgs::CameraInfo& camera_info_msg )
{
// Read calibration data and fill camera_info message:
int width, height;
nh.param(base_name + "/image_width", width, 640);
nh.param(base_name + "/image_height", height, 480);
camera_info_msg.width = width;
camera_info_msg.height = height;
std::string distortion_model;
if (not nh.getParam (base_name + "/distortion_model", distortion_model))
{
ROS_ERROR("Stereo calibration parameters not found!");
}
camera_info_msg.distortion_model = distortion_model;
std::vector<float> camera_matrix;
if (not nh.getParam (base_name + "/camera_matrix/data", camera_matrix))
{
ROS_ERROR("Stereo calibration parameters not found!");
}
for (unsigned int i = 0; i < camera_matrix.size(); i++)
{
camera_info_msg.K[i] = camera_matrix[i];
}
std::vector<float> rectification_matrix;
if (not nh.getParam (base_name + "/rectification_matrix/data", rectification_matrix))
{
ROS_ERROR("Stereo calibration parameters not found!");
}
for (unsigned int i = 0; i < rectification_matrix.size(); i++)
{
camera_info_msg.R[i] = rectification_matrix[i];
}
std::vector<float> projection_matrix;
if (not nh.getParam (base_name + "/projection_matrix/data", projection_matrix))
{
ROS_ERROR("Stereo calibration parameters not found!");
}
for (unsigned int i = 0; i < projection_matrix.size(); i++)
{
camera_info_msg.P[i] = projection_matrix[i];
}
std::vector<float> distortion_coefficients;
if (not nh.getParam (base_name + "/distortion_coefficients/data", distortion_coefficients))
{
ROS_ERROR("Stereo calibration parameters not found!");
}
for (unsigned int i = 0; i < distortion_coefficients.size(); i++)
{
camera_info_msg.D.push_back(distortion_coefficients[i]);
}
}
CJointController::CJointController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh, std::map<std::string,CServo *>& servos) : CController(name,device_p,nh)
{
joints_dirty_=true;
servos_p_=&servos;
std::string topic;
nh.param("controllers/"+name+"/topic", topic, std::string("cmd_joints"));
nh.param("controllers/"+name+"/rate", rate_, 15.0);
joint_sub_ = nh.subscribe<sensor_msgs::JointState>(topic, 10, &CJointController::jointCallback, this);
timer_=nh.createTimer(ros::Duration(1/rate_),&CJointController::timerCallback,this);
}
void srs_env_model::CMap2DPlugin::init(ros::NodeHandle & node_handle)
{
node_handle.param("collision_object_publisher", m_map2DPublisherName, MAP2D_PUBLISHER_NAME );
node_handle.param("collision_object_frame_id", m_map2DFrameId, MAP2D_FRAME_ID );
node_handle.param("min_x_size", m_minSizeX, m_minSizeX);
node_handle.param("min_y_size", m_minSizeY, m_minSizeY);
// Create publisher
m_map2DPublisher = node_handle.advertise<nav_msgs::OccupancyGrid> (m_map2DPublisherName, 100, m_latchedTopics);
}
void init() {
nh_.param("scale_angular", a_scale_, 1.0);
nh_.param("scale_linear", l_scale_, 0.23);
nh_.param("walk_scale", walk_scale_, 0.6);
vel_pub_ = nh_.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
ros::NodeHandle n_private("~");
}
CBatteryController::CBatteryController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) : CController(name,device_p,nh)
{
level_=0;
stat_=0;
std::string topic;
nh.param("controllers/"+name+"/topic", topic, std::string("battery_state"));
nh.param("controllers/"+name+"/rate", rate_, 15.0);
battery_pub_ = nh.advertise<qbo_arduqbo::BatteryLevel>(topic, 1);
timer_=nh.createTimer(ros::Duration(1/rate_),&CBatteryController::timerCallback,this);
}
//! Initialize plugin - called in server constructor
void srs_env_model::CPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
// PERROR( "Initializing PointCloudPlugin" );
// Frame skipping
int fs( m_use_every_nth );
node_handle.param( "pointcloud_frame_skip", fs, 1 );
// m_use_every_nth = (fs >= 1) ? fs : 1;
// Point cloud publishing topic name
node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );
// Point cloud subscribing topic name - try to get it from parameter server if not given
if( m_pcSubscriberName.length() == 0 )
node_handle.param("pointcloud_subscriber", m_pcSubscriberName, SUBSCRIBER_POINT_CLOUD_NAME);
else
m_bSubscribe = true;
// Get FID to which will be points transformed when publishing collision map
node_handle.param("pointcloud_frame_id", m_pcFrameId, m_pcFrameId ); //
// Point cloud limits
node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ);
node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ);
// Create publisher
m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 1, m_latchedTopics);
// If should subscribe, create message filter and connect to the topic
if( m_bSubscribe )
{
// PERROR("Subscribing to: " << m_pcSubscriberName );
// Create subscriber
m_pcSubscriber = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1);
if (!m_pcSubscriber)
{
ROS_ERROR("Not subscribed...");
PERROR( "Not subscirbed to point clouds subscriber...");
}
// Create message filter
m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_pcFrameId, 1);
m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1));
//std::cerr << "SUBSCRIBER NAME: " << m_pcSubscriberName << ", FRAMEID: " << m_pcFrameId << std::endl;
}
// Clear old pointcloud data
m_data->clear();
// PERROR( "PointCloudPlugin initialized..." );
}
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);
}
// constructor
TOea_Planner::TOea_Planner(ros::NodeHandle &n, ros::NodeHandle &private_n, std::string logger_name) : Astar_(logger_name)//: plan_("/move_platform", n)
{
logger_name_ = logger_name;
ros::start();
//load parameters:
private_n.param<std::string>("global_frame_id", global_frame_id, "map");
private_n.param<std::string>("base_frame_id", base_frame_id, "base_footprint");
private_n.param("inflate_map_borders", Astar_.inflate_map_borders_, false);
private_n.param("robot_x_size", Astar_.robot_x_size_, 1.2);//1.2
private_n.param("robot_y_size", Astar_.robot_y_size_, 0.8);//0.8
private_n.param("allow_unknown", Astar_.allow_unknown_, true);//0.8
private_n.param("stop_at_exact_target", Astar_.stop_at_exact_target_, false);
private_n.param("publish_entire_pcd", Astar_.publish_entire_pcd_, false);//0.8
private_n.param("use_frontal_laser", Astar_.use_frontal_laser, true);
private_n.param("use_back_laser", Astar_.use_back_laser, true);
private_n.param("use_localization", use_localization, true);
private_n.param("mark_end_cubes", mark_end_cubes, true);
//TODO: DEBUG THIS
//print parameters
/* if (true)
{
std::cout << BOLDMAGENTA << " Parameters: " << RESET << std::endl;
std::cout << BOLDWHITE << " * global_frame_id: " << RESET << global_frame_id << std::endl;
std::cout << BOLDWHITE << " * base_frame_id: " << RESET << base_frame_id << std::endl;
std::cout << BOLDWHITE << " * robot_x_size: " << RESET << Astar_.robot_x_size_ << std::endl;
std::cout << BOLDWHITE << " * robot_y_size: " << RESET << Astar_.robot_y_size_ << std::endl;
std::cout << BOLDWHITE << " * inflate_map_borders: " << RESET << Astar_.inflate_map_borders_ << std::endl;
std::cout << BOLDWHITE << " * allow_unknown_: " << RESET << Astar_.allow_unknown_ << std::endl;
std::cout << BOLDWHITE << " * stop_at_exact_target: " << RESET << Astar_.stop_at_exact_target_ << std::endl;
std::cout << BOLDWHITE << " * publish_entire_pcd: " << RESET << Astar_.publish_entire_pcd_ << std::endl;
std::cout << BOLDWHITE << " * use_frontal_laser: " << RESET << Astar_.use_frontal_laser << std::endl;
std::cout << BOLDWHITE << " * use_back_laser: " << RESET << Astar_.use_back_laser << std::endl;
std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_closest: " << Astar_.level_closest << RESET << std::endl;
std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_middle: " << Astar_.level_middle << RESET << std::endl;
std::cout << BOLDWHITE << " >>>>>>>>>>>>>>>>>< level_farthest: " << Astar_.level_farthest << RESET << std::endl;
std::cout << BOLDWHITE <<" ****************************************" << RESET << std::endl;
}*/
// subscribers:
goal_sub_ = n.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &TOea_Planner::goalCB, this);
start_pose_sub_ = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/start_pose", 1, &TOea_Planner::start_poseCB, this);
map_sub_ = n.subscribe<nav_msgs::OccupancyGrid>("/map", 100, &TOea_Planner::mapCB, this);
// publishers:
pcd_pub_ = private_n.advertise<sensor_msgs::PointCloud2>("inflation_cloud", 1, true); // inflation point cloud (latched)
oea_path_pub_ = private_n.advertise<oea_msgs::Oea_path>("oea_plan",1); //publishing this topic causes the controller to start
visual_path_pub_ = n.advertise<nav_msgs::Path>("/plan",1, true); //latched topic, same as for the actions -> this way we can have only one topic for visualization
arrows_pub_ = private_n.advertise<visualization_msgs::MarkerArray>("arrows_marker_array", 1, true);
state_pub_ = private_n.advertise<std_msgs::UInt8>("state", 1);
cells_pub_ = private_n.advertise<visualization_msgs::MarkerArray>("cells_marker_array", 1, true);
// services:
ss_ = private_n.advertiseService("IsPoseValid", &TOea_Planner::IsPoseValid, this);
map_received_ = false;
}
SegmentPlane::SegmentPlane(ros::NodeHandle nh, ros::NodeHandle n)
: nh_(nh), rate_(n.param("loop_rate", 10)),
frame_id_(n.param<std::string>("inliers_pc_frame_id", "/inliers_pc_frame"))
{
leaf_size_x_ = n.param("leaf_size_x", 0.3);
leaf_size_y_ = n.param("leaf_size_y", 0.3);
leaf_size_z_ = n.param("leaf_size_z", 0.3);
source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/source_pointcloud"), 1, &SegmentPlane::detectPlaneCallback, this);
inliers_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("inliers_pc_topic_name", "/inliers_pointcloud"), 1);
}
KinectV2Downsampler::KinectV2Downsampler(ros::NodeHandle nh, ros::NodeHandle n)
: nh_(nh), rate_(n.param("loop_rate", 10)),
frame_id_(n.param<std::string>("resized_pc_frame_id", "/resized_pc_frame"))
{
leaf_size_x_ = n.param("leaf_size_x", 0.3);
leaf_size_y_ = n.param("leaf_size_y", 0.3);
leaf_size_z_ = n.param("leaf_size_z", 0.3);
source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/source_pointcloud"), 1, &KinectV2Downsampler::resizeCallback, this);
resized_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("resized_pc_topic_name", "/resized_pointcloud"), 1);
}