int init(PhantomState *s)
{
if(!s)
{
ROS_FATAL("Internal error. PhantomState is NULL.");
return -1;
}
node_ = ros::NodeHandlePtr(new ros::NodeHandle);
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));
// Vertical displacement from base_link to link_0. Defaults to Omni offset.
pnode_->param(std::string("table_offset"), table_offset_, .135);
// Force feedback damping coefficient
pnode_->param(std::string("damping_k"), damping_k_, 0.001);
// On startup device will generate forces to hold end-effector at origin.
pnode_->param(std::string("locked"), locked_, false);
// Check calibration status on start up and calibrate if necessary.
pnode_->param(std::string("calibrate"), calibrate_, false);
//Frame attached to the base of the phantom (NAME/base_link)
base_link_name_ = "base_link";
//Publish on NAME/pose
std::string pose_topic_name = "pose";
pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);
//Publish button state on NAME/button
std::string button_topic = "button";
button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);
//Subscribe to NAME/force_feedback
std::string force_feedback_topic = "force_feedback";
wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);
//Frame of force feedback (NAME/sensable_origin)
sensable_frame_name_ = "sensable_origin";
for (int i = 0; i < 7; i++)
{
std::ostringstream stream1;
stream1 << "link_" << i;
link_names_[i] = std::string(stream1.str());
}
state_ = s;
state_->buttons[0] = 0;
state_->buttons[1] = 0;
state_->buttons_prev[0] = 0;
state_->buttons_prev[1] = 0;
hduVector3Dd zeros(0, 0, 0);
state_->velocity = zeros;
state_->inp_vel1 = zeros; //3x1 history of velocity
state_->inp_vel2 = zeros; //3x1 history of velocity
state_->inp_vel3 = zeros; //3x1 history of velocity
state_->out_vel1 = zeros; //3x1 history of velocity
state_->out_vel2 = zeros; //3x1 history of velocity
state_->out_vel3 = zeros; //3x1 history of velocity
state_->pos_hist1 = zeros; //3x1 history of position
state_->pos_hist2 = zeros; //3x1 history of position
state_->lock = locked_;
state_->lock_pos = zeros;
state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);
return 0;
}
int init()
{
// Use global namespace for node
node_ = ros::NodeHandlePtr(new ros::NodeHandle());
// Get tf_prefix from global namespace
node_->param("tf_prefix", tf_prefix_, std::string(""));
// Use private namespace for parameters
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param("publish_rate", publish_rate_, PUBLISH_RATE);
pnode_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
fixed_frame_id_ = tf::resolve(tf_prefix_, fixed_frame_id_);
pnode_->param("publish_odom_tf", publish_odom_tf_, true);
pnode_->param("crio/ip", crio_ip_, std::string("127.0.0.1"));
pnode_->param("crio/command_port", crio_cmd_port_, std::string("39000"));
pnode_->param("crio/state_port", crio_state_port_, std::string("39001"));
pnode_->param("crio/socket_timeout", socket_timeout_, 10);
VehicleKinematics::Parameters kin_params;
tfScalar minimum_radius_outer;
if (!pnode_->getParam("kinematics/frame_id", kin_params.frame_id))
{
ROS_WARN_STREAM(pnode_->getNamespace() << "/kinematics/frame_id parameter is not set");
}
else
{
kin_params.frame_id = tf::resolve(tf_prefix_, kin_params.frame_id);
}
if (!pnode_->getParam("kinematics/wheelbase", kin_params.wheelbase_length))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/wheelbase parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/track", kin_params.track_width))
{
ROS_FATAL_STREAM( pnode_->getNamespace() << "/kinematics/track parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/rotation_center", kin_params.rotation_center))
{
ROS_WARN_STREAM(
pnode_->getNamespace()
<< "/kinematics/rotation_center parameter is not set. Using default: wheelbase/2 = "
<< kin_params.wheelbase_length / 2);
kin_params.rotation_center = kin_params.wheelbase_length / 2;
}
if (!pnode_->getParam("kinematics/minimum_radius_outer", minimum_radius_outer))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/minimum_radius_outer parameter is not set");
return -1;
}
else
{
kin_params.minimum_radius = minimum_radius_outer - kin_params.track_width / 2;
}
if (!pnode_->getParam("kinematics/steering_ratio", kin_params.steering_ratio))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/steering_ratio parameter is not set");
return -1;
}
// kin_(2.7, 1.626, 1.35);
kin_ = boost::make_shared<VehicleKinematics>(kin_params);
sub_ = node_->subscribe<kut_ugv_msgs::MotionCommandStamped>("motion_command", 200,
&CompactRIOCommunicationNode::motionCommand, this);
odom_pub_ = node_->advertise<nav_msgs::Odometry>("odom", 200);
state_pub_ = node_->advertise<kut_ugv_vehicle::StateStamped>("state", 200);
tf_br_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster);
timeout_ = boost::posix_time::seconds(socket_timeout_);
send_ep_ = udp::endpoint(boost::asio::ip::address::from_string(crio_ip_), std::atoi(crio_cmd_port_.c_str()));
receive_ep_ = udp::endpoint(udp::v4(), std::atoi(crio_state_port_.c_str()));
socket_.open(udp::v4());
deadline_.expires_at(boost::posix_time::pos_infin);
this->deadlineCallback(deadline_, socket_);
return 0;
}
void init()
{
/*
This is the initialize funtion to load configuration parameters from launch file
*/
// Ros Node
node_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
std::cout << "Starting with the following parameters:" << std::endl;
// Settings
node_->param(std::string("publish_rate"), publish_rate_, PUBLISH_RATE);
std::cout << "\t" << "publish_rate = " << publish_rate_ << std::endl;
node_->param(std::string("master1"), master1_, false);
std::cout << "\t" << "master 1 = " << master1_ << std::endl;
node_->param(std::string("master2"), master2_, false);
std::cout << "\t" << "master 2 = " << master2_ << std::endl;
node_->param(std::string("slave"), slave_, false);
std::cout << "\t" << "slave = " << slave_ << std::endl;
node_->param(std::string("popc_enable"), popc_enable_, false);
std::cout << "\t" << "popc_enable = " << popc_enable_ << std::endl;
// Time delay
node_->param(std::string("time_delay"), time_delay_, TIME_DELAY);
std::cout << "\t" << "time_delay = " << time_delay_ << std::endl;
// Subscriber settings
// actual velocity
node_->param(std::string("actual_velocity_src_name"), actual_velocity_src_name_, std::string(""));
if (!actual_velocity_src_name_.empty())
{
node_->param(std::string("actual_velocity_topic_name"), actual_velocity_topic_name_, std::string("velocity"));
std::cout << "\t" << "actual_velocity_topic_name = " << actual_velocity_topic_name_ << std::endl;
actual_velocity_sub_ = node_->subscribe<brl_teleop_msgs::Vel>(actual_velocity_src_name_ + std::string("/") +
actual_velocity_topic_name_, 100,&controller::actualVelocityCallback,this);
}
// Velocity Master - Master
node_->param(std::string("velocity_master_master_src_name"), velocity_master_master_src_name_, std::string(""));
if(!velocity_master_master_src_name_.empty())
{
node_->param(std::string("velocity_master_master_topic_name"), velocity_master_master_topic_name_, std::string("velocity_master_master"));
std::cout << "\t" << "velocity_master_master_topic_name = " << velocity_master_master_topic_name_ << std::endl;
velocity_master_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master_master_src_name_ + std::string("/") + velocity_master_master_topic_name_, 100,
&controller::velocityMasterToMasterCallback,this);
}
// Force Master - Master
node_->param(std::string("force_master_master_src_name"), force_master_master_src_name_, std::string(""));
if(!force_master_master_src_name_.empty())
{
node_->param(std::string("force_master_master_topic_name"), force_master_master_topic_name_, std::string("force_master_master"));
std::cout << "\t" << "force_master_master_topic_name = " << force_master_master_topic_name_ << std::endl;
force_master_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(force_master_master_src_name_ + std::string("/") + force_master_master_topic_name_, 100,
&controller::forceMasterToMasterCallback,this);
}
// Velocity Master1 - Slave
node_->param(std::string("velocity_master1_slave_src_name"), velocity_master1_slave_src_name_, std::string(""));
if(!velocity_master1_slave_src_name_.empty())
{
node_->param(std::string("velocity_master1_slave_topic_name"), velocity_master1_slave_topic_name_, std::string("velocity_master_slave"));
std::cout << "\t" << "velocity_master1_slave_topic_name = " << velocity_master1_slave_topic_name_ << std::endl;
velocity_master1_slave_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master1_slave_src_name_ + std::string("/") + velocity_master1_slave_topic_name_, 100,
&controller::velocityMaster1ToSlaveCallback,this);
}
// Velocity Master2 - Slave
node_->param(std::string("velocity_master2_slave_src_name"), velocity_master2_slave_src_name_, std::string(""));
if(!velocity_master2_slave_src_name_.empty())
{
node_->param(std::string("velocity_master2_slave_topic_name"), velocity_master2_slave_topic_name_, std::string("velocity_master_slave"));
std::cout << "\t" << "velocity_master2_slave_topic_name = " << velocity_master2_slave_topic_name_ << std::endl;
velocity_master2_slave_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master2_slave_src_name_ + std::string("/") + velocity_master2_slave_topic_name_, 100,
&controller::velocityMaster2ToSlaveCallback,this);
}
// Force Slave - Master
node_->param(std::string("force_slave_master_src_name"), force_slave_master_src_name_, std::string(""));
if(!force_slave_master_src_name_.empty())
{
node_->param(std::string("force_slave_master_topic_name"), force_slave_master_topic_name_, std::string("force_slave_master"));
std::cout << "\t" << "force_slave_master_topic_name = " << force_slave_master_topic_name_ << std::endl;
force_slave_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(force_slave_master_src_name_ + std::string("/") + force_slave_master_topic_name_, 100,
&controller::forceSlaveToMasterCallback,this);
}
//PID
node_->param(std::string("gains/P"), Kp_, 0.1);
node_->param(std::string("gains/D"), Kd_, 0.01);
std::cout << "\t" << "Gains = { P: " << Kp_ << ", D: " << Kd_ << "}" << std::endl;
// Publishing settings
velocity_master_master_pub_ = node_->advertise<brl_teleop_msgs::Package>("velocity_master_master", 100);
velocity_master_slave_pub_ = node_->advertise<brl_teleop_msgs::Package>("velocity_master_slave", 100);
force_master_master_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_master_master", 100);
force_slave_master1_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_slave_master1", 100);
force_slave_master2_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_slave_master2", 100);
force_popc_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_popc", 100);
}
