//Initialize parameters
WF_Agent::WF_Agent(ros::NodeHandle nh):
n(nh) //ros::NodeHandle n = nh;
{
//accessing private parameters
ros::NodeHandle n_private("~");
/*linAxis, angAxis variables are used to define which axes of the
*joystick will control the pioneer robot. Also, the parameter server
* was checked for new scalar values for driving the robot. */
// n_private.param("linAxis", linAxis, 1);
// n_private.param("angAxis", angAxis, 0);
n_private.param("vel_topic", vel_topic, std::string("cmd_vel"));
// n_private.param("odom_topic", odom_topic, std::string("odom"));
// n_private.param("laser_topic", laser_topic, std::string("base_scan"));
n_private.param("odom_topic", odom_topic, std::string("pose"));
n_private.param("laser_topic", laser_topic, std::string("scan"));
n_private.param("dist_topic", dist_topic, std::string("dist"));
n_private.param("motor_state_topic", motor_state_topic, std::string("cmd_motor_state")); //pioneer stuff
/*creating a publisher that will advertise the cmd_vel(command velocity
topic of the pioneer robot. */
vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1);
vis_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 0);
mot_pub = n.advertise<p2os_driver::MotorState>(motor_state_topic.c_str(),1); //pioneer stuff
/*subscribing to the joystick topic for the input to drive the robot
*If the node is slow in processing incoming messages on the joystick topic,
* up to 10 messages will be buffered before any are lost. */
odom_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.c_str(), 10, &WF_Agent::odom_cb, this);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_topic.c_str(), 10, &WF_Agent::laser_cb, this);
wall_dist_sub = n.subscribe<wf::Dist>(dist_topic.c_str(), 10, &WF_Agent::dist_cb, this);
}
Experiment::Experiment(ros::NodeHandle n):n_(n)
{
ros::NodeHandle n_private("~");
n_private.param("num_reps", num_reps_, 1000);
n_private.param("freq", freq_, 1.0);
n_private.param("start_x", start_x_, -5.0);
n_private.param("start_y", start_y_, 5.0);
n_private.param("goal_radius", radius_, 0.5);
n_private.param("learn", learn_, true);
n_private.param("learning_rate", learning_rate_, 0.5);
n_private.param("discount_factor", discount_factor_, 0.5);
n_private.param("max_explore", max_explore_, 8);
cnt_rep_ = 0;
move_stopped_ = false;
mode_ = MODE_REP_START;
states_ = new States(n);
actions_ = new Actions(n);
qobj_ = new QLearner(actions_->GetNumActions(),
states_->GetNumStates(), learning_rate_, discount_factor_, learn_,
max_explore_);
bool_sub_ = n.subscribe("/move_done", 1, &Experiment::bool_cb, this);
// ros::Subscriber odom_sub_ = n.subscribe("/odom", 10, odom_cb);
client_ = n.serviceClient<stage_light_ml::move_robot>("/move_robot");
timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this);
}
KeyboardTeleopNode()
{
pub_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1000);
cmdjs_.name.clear();
cmdjs_.name.push_back("joint_1");
cmdjs_.name.push_back("joint_2");
cmdjs_.name.push_back("joint_3");
cmdjs_.name.push_back("joint_4");
cmdjs_.name.push_back("joint_5");
cmdjs_.name.push_back("joint_6");
cmdjs_.position.clear();
for(int i=0;i<6;++i){
cmdjs_.position.push_back(0.0f);
}
// needed header time info, or the rviz can not display the model states
cmdjs_.header.stamp= ros::Time::now();
pub_.publish(cmdjs_);
ros::NodeHandle n_private("~");
}
int main(int argc,char ** argv){
ros::init(argc, argv, "ml_ndt");
ros::NodeHandle n;
ros::NodeHandle n_private("~");
MlNdt ndt(n,n_private);
ros::spin();
return 0;
}
KeyboardTeleopArm::KeyboardTeleopArm()
{
ros::NodeHandle n_private("~");
n_private.param("joint_angular_step", joint_angular_step_, 0.0174);
n_private.param("gripper_angular_step", gripper_angular_step_, 0.1);
joint_pos_pub_ = xm_nh_.advertise<xm_msgs::xm_JointPos>("joint_pos_cmd", 1000);
gripper_pos_pub_ = xm_nh_.advertise<std_msgs::Float64>("gripper_joint/command", 1000);
}
ErraticKeyboardTeleopNode() {
pub_ = n_.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
ros::NodeHandle n_private ("~");
n_private.param ("walk_vel", walk_vel_, 0.4);
n_private.param ("run_vel", run_vel_, 0.7);
n_private.param ("yaw_rate", yaw_rate_, 0.4);
n_private.param ("yaw_rate_run", yaw_rate_run_, 0.8);
}
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("~");
}
void init()
{
cmd.data = 0;
vel_pub_r_ = n_.advertise<std_msgs::Float64>("/wsg_50_gr/command", 1);
vel_pub_l_ = n_.advertise<std_msgs::Float64>("/wsg_50_gl/command", 1);
ros::NodeHandle n_private("~");
n_private.param("open_increment", open_increment, 0.001);
}
ErraticKeyboardTeleopNode()
{
pub_ = n_.advertise<geometry_msgs::Twist>("desired_cmd_vel", 1);
//pub_ = n_.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel_, 0.5);
n_private.param("run_vel", run_vel_, 1.0);
n_private.param("yaw_rate", yaw_rate_, 1.0);
n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
}
QLearner::QLearner(ros::NodeHandle nh)
{
n_ = nh;
ros::NodeHandle n_private("~");
n_private.param("num_states", num_states_, 8);
n_private.param("num_actions", num_actions_, 3);
n_private.param("learning_rate", learning_rate_, 0.3);
n_private.param("discount_factor", discount_factor_, 0.5);
n_private.param("temp_const", temp_const_, 5.0);
n_private.param("temp_alpha", temp_alpha_, 0.85);
temp_cnt_ = 0;
temp_ = temp_const_ * pow(temp_alpha_, temp_cnt_);
size_array_ = num_actions_ * num_states_;
srand ( time(NULL) );
Init();
if (n_private.hasParam("qarray"))
{
learn_ = false;
// Get the qtable
XmlRpc::XmlRpcValue qtable;
n_private.getParam("qarray", qtable);
if (qtable.getType() != XmlRpc::XmlRpcValue::TypeArray)
ROS_ERROR("Error reading footsteps/x from config file.");
int size;
try
{
size = qtable.size();
if (size != (num_states_ * num_actions_))
{
ROS_ERROR("Size of array does not match num_states * num_actions = %d,\
exiting.", num_states_ * num_actions_);
exit(0);
}
}
catch (const XmlRpc::XmlRpcException e)
{
ROS_ERROR("No table available, exiting.");
exit(0);
}
// create qarray set
for(int i=0; i < size; i++)
{
q_array_.push_back((double)qtable[i]);
}
}
MoveSimple::MoveSimple(ros::NodeHandle nh):n_(nh)
{
ros::NodeHandle n_private("~");
n_private.param("max_lin_vel", max_lin_vel_, 0.5);
n_private.param("max_ang_vel", max_ang_vel_, 1.0);
move_cmd_sub_ = n_.subscribe("move_cmd", 1, &MoveSimple::cb_cmd, this);
odom_sub_ = n_.subscribe("base_pose_ground_truth", 1, &MoveSimple::cb_odom, this);
vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
move_done_pub_ = n_.advertise<std_msgs::Bool>("move_done", 1);
state_ = MOVE_NONE;
}
void init()
{
cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel, 0.5);
n_private.param("run_vel", run_vel, 1.0);
n_private.param("yaw_rate", yaw_rate, 1.0);
n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
}
ManualCalibration()
{
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel_, 0.5);
n_private.param("run_vel", run_vel_, 1.0);
n_private.param("yaw_rate", yaw_rate_, 1.0);
n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
tx=-0.3483; ty=-0.01316162; tz=0.643236;
tf::Quaternion quat(0.228405, 0.326475, -0.502927, 0.767014);
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}
void init()
{
cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
gripper.data = 0;
joy_sub_ = n_.subscribe<sensor_msgs::Joy>("/teleop_joy",10,&TeleopUAVJoy::JoyCallback,this);
vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
grip_pub_ = n_.advertise<std_msgs::Float64>("/r_gripper_controller/command",1);
if(!n_.getParam("teleopUGV",teleopUGV))
puts("fail to load the param");
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel, 1.0);
n_private.param("run_vel", run_vel, 8.0);
n_private.param("yaw_rate", yaw_rate, 0.5);
n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
n_private.param("vertical_vel", vertical_vel, 2.0);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "PCL_Updater");
ros::NodeHandle n;
ros::NodeHandle n_private("~");
n_private.param("period", period, 0.1);
ros::Subscriber PCLSub = n.subscribe("/cloud_out", 1000, PCL_callback);
ros::Timer timer = n.createTimer(ros::Duration(period), RefreshData);
PCL_Pub = n.advertise<sensor_msgs::PointCloud>("/Assembled_PCL", 10);//Publish PointCloud
PCL2_Pub = n.advertise<sensor_msgs::PointCloud2>("/Assembled_PCL2", 10);//Publish PointCloud2
ros::spin();
return 0;
}
void KeyboardTeleoperation::init() {
baseCommand.linear.x = 0;
baseCommand.linear.y = 0;
baseCommand.angular.z = 0;
baseCommandPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
baseOdometrySubscriber = n.subscribe("odom", 1, &KeyboardTeleoperation::baseOdometryCallback, this); // subscribing for joint states
ros::NodeHandle n_private("~");
n_private.param("translationalSpeed", translationalSpeed, 0.1);
n_private.param("rotationalSpeed", rotationalSpeed, 0.1);
n_private.param("incrementRate", incrementRate, 0.1);
n_private.param("maxTraveledDistance", maxTraveledDistance, 1.0);
experimentNumber = 0;
experimentIsRunning = false;
}
void init()
{
//header - this is impt
cmd.header.frame_id = "/torso_lift_link";
//Clear out our cmd - these values are roundabout initials
cmd.pose.position.x=0.6;
cmd.pose.position.y=-0.2;
cmd.pose.position.z=-0.1;
cmd.pose.orientation.x=-0.00244781865415;
cmd.pose.orientation.y=-0.548220284495;
cmd.pose.orientation.z=0.00145617884538;
cmd.pose.orientation.w=0.836329126239;
pose_pub_ = n_.advertise<geometry_msgs::PoseStamped>("r_cart/command", 1);
ros::NodeHandle n_private("~");
}
LightSensor::LightSensor(ros::NodeHandle nh)
{
double xdist, ydist;
n_ = nh;
ros::NodeHandle n_private("~");
n_private.param("xdist", xdist, 8 / 0.0254);
n_private.param("ydist", ydist, 6 / 0.0254);
angle_ = atan2(ydist, xdist);
sensor_hypotenuse_ = sqrt(xdist * xdist + ydist * ydist);
flls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("flls", 1);
frls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("frls", 1);
rlls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("rlls", 1);
rrls_pub_ = n_.advertise<phidgets_ros::Float64Stamped>("rrls", 1);
odom_sub_ = n_.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 10, &LightSensor::cb_odom, this);
timer_ = n_.createTimer(ros::Duration(0.02), &LightSensor::cb_tmr, this);
}
Actions::Actions(ros::NodeHandle nh)
{
n_ = nh;
ros::NodeHandle n_private("~");
n_private.param("num_actions", num_actions_, 3);
n_private.param("ang_vel_lim", ang_vel_lim_, 1.0);
n_private.param("lin_vel", lin_vel_, 0.3);
vel_pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
tmr_vel_ = n_.createTimer(ros::Duration(1 / 20), &Actions::timer_cb, this);
ang_vels_ = std::vector<double>(num_actions_);
ang_inc_ = 2 * ang_vel_lim_ / (num_actions_ - 1);
for (int i = 0; i < num_actions_; i++)
ang_vels_[i] = -ang_vel_lim_ + i * ang_inc_;
vel_msg_.linear.x = 0.0;
vel_msg_.angular.z = 0.0;
}
TrackingApp::TrackingApp(const ros::NodeHandle& nodeh)
{
// Init attributes
this->nh = nodeh;
// Setup some ROS stuff
statusSub = nh.subscribe("sensor_status", 1, &TrackingApp::statusCallback, this);
cmdPub = nh.advertise<std_msgs::UInt16>("cmd_mode", 1);
// Parameters
ros::NodeHandle n_private("~");
int tmpTime;
n_private.param("wantedMowingTime", tmpTime, 60 * 60);
ROS_INFO("Param: wantedMowingTime: [%d]", tmpTime);
wantedMowingTime = ros::Duration(tmpTime);
state = HVA_TRK_STATE_IDLE;
logCounter = 0;
}
void init(){
ls_sub_ = n_.subscribe("ls_info", 1, &TeleopUAVController::laserInfoCallback, this);
cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
vel_pub_ = n_.advertise<geometry_msgs::Twist>("/uav1/cmd_vel", 1);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel, 1.0);
n_private.param("run_vel", run_vel, 1.0);
n_private.param("yaw_rate", yaw_rate, 1.0);
n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
walk_vel = 1.0;
run_vel = 1.0;
yaw_rate= 1.0;
yaw_rate_run = 1.5;
number = 0;
}
WheelMap::WheelMap(const ros::NodeHandle& nodeh)
{
// Init attributes
this->nh = nodeh;
// Setup some ROS stuff
map_pub = nh.advertise<nav_msgs::OccupancyGrid>("map", 1);
ROS_INFO("nh.subscribe, wheel_encoder\n");
encoderSub = nh.subscribe("wheel_encoder", 1, &WheelMap::encoderCallback, this);
//ros::spin();
// Allocate the map
mapData = new int8_t[AM_MAP_WIDTH*AM_MAP_HEIGHT];
memset(mapData, AM_LM_FREE_SPACE, sizeof(mapData));
// Setup the header & info statically...
theHeader.frame_id = "map2";
theInfo.resolution = 0.01;
theInfo.width = AM_MAP_WIDTH;
theInfo.height = AM_MAP_HEIGHT;
theInfo.origin.position.x = -10.0;
theInfo.origin.position.y = -10.0;
theInfo.origin.position.z = 0.0;
// Parameters
ros::NodeHandle n_private("~");
double def_offset = 0.0;
n_private.param("offset", offset, def_offset);
ROS_INFO("Offset: %f", offset);
double def_scaleFactor = 15.0;
n_private.param("scale_factor", scaleFactor, def_scaleFactor);
ROS_INFO("Scale factor: %f", scaleFactor);
}
Experiment::Experiment(ros::NodeHandle n):n_(n)
{
ros::NodeHandle n_private("~");
n_private.param("num_reps", num_reps_, 100);
n_private.param("freq", freq_, 2.0);
n_private.param("goal_radius", goal_radius_, 0.5);
n_private.param("start_radius", start_radius_, 5.0);
n_private.param("goalx", goalx_, 0.0);
n_private.param("goaly", goaly_, 0.0);
n_private.param("bounds/tlx", bounds_[0], -8.0);
n_private.param("bounds/tly", bounds_[1], 8.0);
n_private.param("bounds/brx", bounds_[2], 8.0);
n_private.param("bounds/bry", bounds_[3], -8.0);
if (n_private.hasParam("qarray"))
learn_ = false;
else
learn_ = true;
srand ( time(NULL) );
cnt_rep_ = 0;
cnt_timesteps_ = 0;
mode_ = MODE_REP_START;
states_ = new States(n);
actions_ = new Actions(n);
qobj_ = new QLearner(n);
//lc_ = new LearningCurve();
path_msg_.header.frame_id = "odom";
move_pub_ = n_.advertise<geometry_msgs::Pose>("move_cmd", 1);
path_pub_ = n_.advertise<nav_msgs::Path>("path", 1);
path_final_pub_ = n_.advertise<nav_msgs::Path>("path_final", 1);
lc_pub_ = n_.advertise<std_msgs::Float64>("learning_times", 1);
bool_sub_ = n_.subscribe("move_done", 1, &Experiment::bool_cb, this);
odom_sub_ = n.subscribe("base_pose_ground_truth", 10, &Experiment::odom_cb, this);
timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this);
}
IMUPosition::IMUPosition(const ros::NodeHandle& nodeh)
{
// Init attributes
nh = nodeh;
// Setup some ROS stuff
odomSub = nh.subscribe("odom", 1, &IMUPosition::odomCallback, this);
joySub = nh.subscribe("nano2", 1, &IMUPosition::joyCallback, this);
imuSub = nh.subscribe("imu", 1, &IMUPosition::imuCallback, this);
imuResetSub = nh.subscribe("imu_reset", 1, &IMUPosition::imuResetCallback, this);
posePub = nh.advertise<geometry_msgs::PoseStamped>("pose_combined", 10);
poseArrayPub = nh.advertise<geometry_msgs::PoseArray>("estimated_poses", 1);
helperPub = nh.advertise<visualization_msgs::Marker>("helpers", 25);
// Initialize the intial pose
robotPose.pose.position.x = 0.0;
robotPose.pose.position.y = 0.0;
robotPose.pose.position.z = 0.0;
xpos = 0.0;
ypos = 0.0;
zpos = 0.0;
heading = 0.0;
qyaw = tf::createQuaternionFromYaw(heading);
dxTot = 0;
dyTot = 0;
dhTot = 0;
lastOdoHeading = 0;
newOdo = false;
lastHeading = 0;
gotFirst = false;
// Parameters
ros::NodeHandle n_private("~");
posTopicName = "odom_combined";
n_private.param<std::string>("posTopicName", posTopicName, "odom_combined");
ROS_INFO("Param: posTopicName: %s", posTopicName.c_str());
odomPub = nh.advertise<nav_msgs::Odometry>(posTopicName, 25);
// Only use odo?
onlyOdo = 1;
n_private.param("onlyOdo", onlyOdo, 1);
ROS_INFO("Param: onlyOdo: [%d]", onlyOdo);
waitingForRelease = false;
joyDisabled = true;
// Default
odom.pose.pose.position.x = 0.0;
odom.pose.pose.position.y = 0.0;
odom.pose.pose.position.z = 0.0;
tf::Quaternion qyaw = tf::createQuaternionFromYaw(0.0);
odom.pose.pose.orientation.x = qyaw.x();
odom.pose.pose.orientation.y = qyaw.y();
odom.pose.pose.orientation.z = qyaw.z();
odom.pose.pose.orientation.w = qyaw.w();
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = 0.0;
setDefaultValues();
smoothHeadingMode = false;
imuOffsetYaw = 0.0;
imuDeltaYaw = 0.0;
lastImuYaw = 0.0;
newImu = false;
imuOffsetYaw = 0.0;
lastTwistZ = 0.0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "uvc_cam");
ros::NodeHandle n;
ros::NodeHandle n_private("~");
std::string device;
std::string out_topic;
n_private.param<std::string>("device", device, "/dev/video0");
n_private.param<std::string>("topic", out_topic, "/camera/image");
int width, height, fps, modetect_lum, modetect_count;
n_private.param("width", width, 640);
n_private.param("height", height, 480);
n_private.param("fps", fps, 30);
n_private.param("motion_threshold_luminance", modetect_lum, 100);
n_private.param("motion_threshold_count", modetect_count, -1);
ros::Publisher pub = n.advertise<sensor_msgs::Image>(out_topic.c_str(), 1);
ROS_INFO("opening uvc_cam at %dx%d, %d fps", width, height, fps);
uvc_cam::Cam cam(device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
cam.set_motion_thresholds(modetect_lum, modetect_count);
ros::Time t_prev(ros::Time::now());
int count = 0, skip_count = 0;
while (n.ok())
{
unsigned char *frame = NULL;
uint32_t bytes_used;
int buf_idx = cam.grab(&frame, bytes_used);
if (count++ % fps == 0)
{
ros::Time t(ros::Time::now());
ros::Duration d(t - t_prev);
ROS_INFO("%.1f fps skip %d", (double)fps / d.toSec(), skip_count);
t_prev = t;
}
if (frame)
{
sensor_msgs::Image image;
image.header.stamp = ros::Time::now();
image.encoding = sensor_msgs::image_encodings::RGB8;
image.height = height;
image.width = width;
image.step = 3 * width;
image.set_data_size( image.step * image.height );
/*
uint8_t* bgr = &(image.data[0]);
for (uint32_t y = 0; y < height; y++)
for (uint32_t x = 0; x < width; x++)
{
// hack... flip bgr to rgb
uint8_t *p = frame + y * width * 3 + x * 3;
uint8_t *q = bgr + y * width * 3 + x * 3;
q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
}
*/
memcpy(&image.data[0], frame, width * height * 3);
pub.publish(image);
cam.release(buf_idx);
}
else
skip_count++;
}
return 0;
}
RangeSteering::RangeSteering(const ros::NodeHandle& nodeh)
{
// Init attributes
nh = nodeh;
// Parameters
ros::NodeHandle n_private("~");
// Setup some ROS stuff
joySub = nh.subscribe("nano2", 1, &RangeSteering::joyCallback, this);
rangeSub = nh.subscribe("uwb", 1, &RangeSteering::rangeCallback, this);
odomSub = nh.subscribe("odom", 1, &RangeSteering::odomCallback, this);
odomCombinedSub = nh.subscribe("odom_combined", 1, &RangeSteering::odomCombinedCallback, this);
//~ beaconPosSub = nh.subscribe("beacon_pos", 1, &RangeSteering::beaconPosCallback, this);
beaconPub = nh.advertise<visualization_msgs::Marker>("helpers", 1);
cmdPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
modePub = nh.advertise<std_msgs::UInt16>("cmd_mode", 1);
statusSub = nh.subscribe("sensor_status", 1, &RangeSteering::statusCallback, this);
coveragePub = nh.advertise<nav_msgs::Path>("coverage_plan", 1);
// Special simulation noise
std::string defRobotId = "DECA0100-100";
n_private.param("robotId", robotId, defRobotId);
ROS_INFO("Param: robotId: [%s]", robotId.c_str());
addBeacons = true;
// Left
beaconList[0].id = "DECA0100-101";
beaconList[0].xpos = 0.0;
beaconList[0].ypos = 0.50;
beaconList[0].zpos = 0.0;
beaconList[0].heading = 0.0;
beaconList[0].range = 0.0;
// Rigth
beaconList[1].id = "DECA0100-102";
beaconList[1].xpos = 0.0;
beaconList[1].ypos = -0.50;
beaconList[1].zpos = 0.0;
beaconList[1].heading = 0.0;
beaconList[1].range = 0.0;
/* Local positioning stuff*/
bearingEst = 0;
minRange = 10000;
dockStart = 1.0; /* When to enter local docking mode */
newRange = false;
doControl = false;
radius = 1.0;
radiusInc = 0.15;
speed = 1.0;
turnSpeed = 1.0;
leftRange = 0;
rightRange = 0;
waitingForRelease = false;
joyDisabled = true;
dockStarted = false;
seekDockStart = false;
followDirection = 1;
kpDock = 1.0;
kdDock = 9.6;
kiDock = 0.00;
kp = 0.7;
kd = 7.25;
ki = 0.20;
numInterBeaconSamples = 0;
/* How much before the baseline between home beacons shall it stop? */
baselineOffset = 0.5;
rangeHome = 0;
errOld = 0.0;
passed180 = false;
state = STATE_START_FOLLOW_RADIUS;
rangeState = STATE_DETECT_BEACONS;
}
int main(int argc, char **argv)
{
// Initialize ROS
ros::init(argc, argv, "am_mpu9150_node");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1);
ros::Rate loop_rate(50);
sensor_msgs::Imu imu;
imu.header.frame_id = "imu";
geometry_msgs::Vector3Stamped euler;
euler.header.frame_id = "imu_euler";
// Init the sensor the values are hardcoded at the local_defaults.h file
int opt, len;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
int verbose = 0;
std::string accelFile;
std::string magFile;
// Parameters from ROS
ros::NodeHandle n_private("~");
int def_i2c_bus = 2;
n_private.param("i2cbus", i2c_bus, def_i2c_bus);
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS)
{
ROS_ERROR("am_mpu9150: Imu Bus problem");
return -1;
}
int def_sample_rate = 50;
n_private.param("samplerate", sample_rate, def_sample_rate);
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE)
{
ROS_ERROR("am_mpu9150: Sample rate problem");
return -1;
}
loop_rate = sample_rate;
int def_yaw_mix_factor = 0;
n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor);
if (yaw_mix_factor < 0 || yaw_mix_factor > 100)
{
ROS_ERROR("am_mpu9150: yawmixfactor problem");
return -1;
}
std::string defAccelFile = "./accelcal.txt";
n_private.param("accelfile", accelFile, defAccelFile);
std::string defMagFile = "./magcal.txt";
n_private.param("magfile", magFile, defMagFile);
unsigned long loop_delay;
mpudata_t mpu;
// Initialize the MPU-9150
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
{
ROS_ERROR("am_mpu9150::Failed init!");
exit(1);
}
//load_calibration(0, accelFile);
//load_calibration(1, magFile);
memset(&mpu, 0, sizeof(mpudata_t));
vector3d_t e;
quaternion_t q;
while (ros::ok())
{
std::stringstream ss;
if (mpu9150_read(&mpu) == 0)
{
// ROTATE The angles correctly...
quaternionToEuler(mpu.fusedQuat, e);
e[VEC3_Z] = -e[VEC3_Z];
eulerToQuaternion(e, q);
// FUSED
imu.header.stamp = ros::Time::now();
imu.orientation.x = q[QUAT_X];
imu.orientation.y = q[QUAT_Y];
imu.orientation.z = q[QUAT_Z];
imu.orientation.w = q[QUAT_W];
// GYRO
double gx = mpu.rawGyro[VEC3_X];
double gy = mpu.rawGyro[VEC3_Y];
double gz = mpu.rawGyro[VEC3_Z];
gx = gx * gyroScaleX;
gy = gy * gyroScaleY;
gz = gz * gyroScaleZ;
imu.angular_velocity.x = gx;
imu.angular_velocity.y = gy;
imu.angular_velocity.z = gz;
// ACCEL
imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
// EULER
euler.header.stamp = imu.header.stamp;
euler.vector.x = mpu.fusedEuler[VEC3_Y];
euler.vector.y = mpu.fusedEuler[VEC3_X];
euler.vector.z = -mpu.fusedEuler[VEC3_Z];
// Publish the messages
imu_pub.publish(imu);
imu_euler_pub.publish(euler);
//ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
TP8::TP8(const double map_resolution, const double map_length,
const double map_border, const uint delta_save,
const double max_lin_vel, const double max_ang_vel,
std::string robotname):
_MAP_RESOLUTION(map_resolution), _MAP_LENGTH(map_length),
_MAP_BORDER(map_border), _DELTA_SAVE(delta_save),
_lin_vel(0.0), _ang_vel(0.0), _avoid(false), _rotating(false),
_rotate_left(false), _MAX_LIN_VEL(max_lin_vel), _MAX_ANG_VEL(max_ang_vel),
_odom_updated(false), _odom_first_update(true), _iteration(0)
{
// Specify markers positions
_markers_wpos[0].x = -8.0; _markers_wpos[0].y = 4.0;
_markers_wpos[1].x = -8.0; _markers_wpos[1].y = -4.0;
_markers_wpos[2].x = -3.0; _markers_wpos[2].y = -8.0;
_markers_wpos[3].x = 3.0; _markers_wpos[3].y = -8.0;
_markers_wpos[4].x = 8.0; _markers_wpos[4].y = -4.0;
_markers_wpos[5].x = 8.0; _markers_wpos[5].y = 4.0;
_markers_wpos[6].x = 3.0; _markers_wpos[6].y = 8.0;
_markers_wpos[7].x = -3.0; _markers_wpos[7].y = 8.0;
// Open file to store robot poses
_outfile.open("Output.txt");
// Write file header
_outfile << "Estimated and real pose of the robot\n\n"
<< "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n";
//
// Update/create map related variables
//
// We will create an image with the map wich contains the original map plus
// a border around it, so as to allow estimates outside of the original
// map.
// Read original map and resize it to our resolution
std::string map_file_path(getenv("HOME"));
map_file_path += "/ros/worlds/stage/cave.png";
cv::Mat org_full_size_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR);
if( org_full_size_map.data == 0 )
std::cerr << "Error reading map, program will shutdown!" << std::endl;
_org_map.create(ceil(_MAP_LENGTH/_MAP_RESOLUTION),
ceil(_MAP_LENGTH/_MAP_RESOLUTION),
CV_8UC3);
cv::resize(org_full_size_map, _org_map,
cv::Size(ceil(_MAP_LENGTH/_MAP_RESOLUTION),
ceil(_MAP_LENGTH/_MAP_RESOLUTION)),
0, 0, cv::INTER_AREA );
// This will be our actual map, with the border around it.
_map.create(ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION),
ceil((_MAP_LENGTH+_MAP_BORDER)/_MAP_RESOLUTION),
CV_8UC3);
cv::copyMakeBorder(_org_map, _map,
floor((_map.size().height-_org_map.size().height)/2),
ceil((_map.size().height-_org_map.size().height)/2),
floor((_map.size().width-_org_map.size().width)/2),
ceil((_map.size().width-_org_map.size().width)/2),
cv::BORDER_CONSTANT,
cv::Scalar(255,255,255));
// Create window for the map
cv::namedWindow("Debug", 0);
// Initialize random number generator
_rng(time(NULL));
/// Initilize Kalman filter related variables
// The state holds X, Y and Theta estimate of the robot, all of type double.
_ekf._state.create(3, 1);
// Covariance matrix associated with the robot movement
_ekf._V = (cv::Mat_<double>(3,3) << 0.10*0.10, 0.00, 0.00,
0.00, 0.03*0.03, 0.00,
0.00, 0.00, 0.20*0.20);
// Process dynamics jacobian regarding the state
// Only elements (1,3) and (2,3) need to be updated, so we set the correct
//values for the other ones now.
_ekf._Fx = (cv::Mat_<double>(3,3) << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
// Process dynamics jacobian regarding the inputs
// Only elements (0,0), (0,1), (1,0) and (1,1) need to be updated, so we set
// the correct the other ones now.
_ekf._Fv = (cv::Mat_<double>(3,3) << 0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 1.0);
// Process covariance (confidence associated with current state)
// (It is called Σ in the theoretic material)
_ekf._P = cv::Mat::zeros(3, 3, CV_64F);
// Get parameters
ros::NodeHandle n_private("~");
n_private.param("min_front_dist", _min_front_dist, 1.0);
n_private.param("stop_front_dist", _stop_front_dist, 0.6);
std::cout << "Random navigation with obstacle avoidance and EKF-based"
<< " localization\n---------------------------" << std::endl;
/// Setup subscribers
_sub_real_pose = _nh.subscribe(robotname + "/base_pose_ground_truth", 1,
&TP8::realPoseCallback, this);
_sub_odom = _nh.subscribe(robotname + "/odom", 10, &TP8::odomCallback, this);
_sub_laser = _nh.subscribe(robotname + "/base_scan", 1, &TP8::laserCallback, this);
_sub_markers = _nh.subscribe(robotname + "/markers", 1, &TP8::markersCallback, this);
/// Setup publisher for linear and angular velocity
_vel_pub = _nh.advertise<geometry_msgs::Twist>(robotname + "/cmd_vel", 1);
/// Wait until we get an initial robot pose (from odometry)
ros::Rate cycle(10.0); // Cycle rate
while( _odom_updated == false )
{
ros::spinOnce();
cycle.sleep();
}
/// Stop the robot (if not stopped already)
_vel_cmd.linear.x = _lin_vel;
_vel_cmd.angular.z = _ang_vel;
_vel_pub.publish(_vel_cmd);
}
/**
* Main function
* Navigate randomly using distance sensor data and Particle Filter-based
* localization.
*/
int main(int argc, char** argv)
{
std::string robot_name = "/robot_0";
// Open file to store robot poses
outfile.open("Output.txt");
// Write file header
outfile << "Estimated and real pose of the robot\n\n"
<< "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n";
//
// Create map related variables
//
// We will create an image with the map wich contains the original map plus
// a border around it, so as to allow estimates outside of the original
// map.
std::string map_file_path(getenv("HOME"));
map_file_path += "/ros/worlds/stage/cave.png";
cv::Mat cave_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR);
if( cave_map.data == 0 )
{
std::cerr << "Error reading map!" << std::endl;
return -1;
}
// Read original map and resize it to our resolution
cv::resize(cave_map, map, map.size(), 0, 0, cv::INTER_AREA );
// Read original map and resize it to our resolution
// We need the original map, black and white, so that it is faster to find
// occupied cells
cv::resize(cv::imread(map_file_path, CV_LOAD_IMAGE_GRAYSCALE),
org_map,
org_map.size(),
0, 0, cv::INTER_AREA );
// We need a temporary map
tmp_map = map.clone();
// Create window for the map
cv::namedWindow("Debug", 0);
//
// Particle filter related variables with their initial values
//
// This variable will hold our final estimation at each iteration
geometry_msgs::Pose2D pose_estimate;
// Initialize particles with uniform random distribution across all space
cv::randu( particles_x,
-MAP_LENGTH/2.0+SAFETY_BORDER,
MAP_LENGTH/2.0-SAFETY_BORDER); // X
cv::randu( particles_y,
-MAP_LENGTH/2.0+SAFETY_BORDER,
MAP_LENGTH/2.0-SAFETY_BORDER); // Y
cv::randu( particles_theta, -CV_PI, CV_PI ); // Theta
// Re-add particles that are outside of the map or inside an obstacle, until
// it is in a free space inside the map.
for(int n = 0; n < NUM_PARTICLES; n++)
{
while( ( (particles_x(n,0) > MAP_LENGTH/2.0) ||
(particles_x(n,0) < -MAP_LENGTH/2.0) ||
(particles_y(n,0) > MAP_LENGTH/2.0) ||
(particles_y(n,0) < -MAP_LENGTH/2.0) ) ||
(org_map.at<uchar>(
cvRound(org_map.size().height/2.0
- particles_y(n,0)/MAP_RESOLUTION),
cvRound(org_map.size().width/2.0
+ particles_x(n,0)/MAP_RESOLUTION)) < 127) )
{
particles_x(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER);
particles_y(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER);
}
}
// Init ROS
ros::init(argc, argv, "tp7");
// ROS variables/objects
ros::NodeHandle nh; // Node handle
// Get parameters
ros::NodeHandle n_private("~");
n_private.param("min_front_dist", min_front_dist, 1.0);
n_private.param("stop_front_dist", stop_front_dist, 0.6);
std::cout << "Random navigation with obstacle avoidance and particle filter"
<< " based localization\n---------------------------" << std::endl;
/// Setup subscribers
// Odometry
ros::Subscriber sub_odom = nh.subscribe(robot_name + "/odom", 10, odomCallback);
// Real, error-free robot pose (for debug purposes only)
ros::Subscriber sub_real_pose = nh.subscribe(robot_name + "/base_pose_ground_truth", 1, realPoseCallback);
// Laser scans
ros::Subscriber sub_laser = nh.subscribe(robot_name + "/base_scan", 1, laserCallback);
// Markers detected
ros::Subscriber sub_markers = nh.subscribe(robot_name + "/markers", 1, markersCallback);
// Setup publisher for linear and angular velocity
vel_pub = nh.advertise<geometry_msgs::Twist>(robot_name + "/cmd_vel", 1);
ros::Rate cycle(10.0); // Cycle rate
// Wait until we get the robot pose (from odometry)
while( odom_updated == false )
{
ros::spinOnce();
cycle.sleep();
}
// Stop the robot (if not stopped already)
vel_cmd.angular.z = 0;
vel_cmd.linear.x = 0;
vel_pub.publish(vel_cmd);
// Infinite loop (will call the callbacks whenever information is available,
// until ros::shutdown() is called.
ros::spin();
// If we are quitting, stop the robot
vel_cmd.angular.z = 0;
vel_cmd.linear.x = 0;
vel_pub.publish(vel_cmd);
// Close file
outfile.close();
// Store final map
cv::imwrite("mapa.png", map);
return 1;
}
/**
* Main function
* Controls the robot using the keyboard keys and outputs posture and velocity
* related information.
*/
int main(int argc, char** argv)
{
// Open file to store robot poses
outfile.open("Postures.txt");
// Create a window to show the map
cv::namedWindow("Mapa", 0);
//
// Create map related variables
//
map.create(ceil(MAP_HEIGHT/MAP_RESOLUTION),
ceil(MAP_WIDTH/MAP_RESOLUTION),
CV_8UC1);
// Set the initial map cell values to "undefined"
map.setTo(127);
//
// Create robot related objects
//
// Linear and angular velocities for the robot (initially stopped)
double lin_vel=0, ang_vel=0;
double last_ang_vel = DEG2RAD(15);
// Navigation variables
bool avoid, new_rotation = false;
double stop_front_dist, min_front_dist;
// Random number generator (used for direction choice)
CvRNG rnggstate = cvRNG(0xffffffff);
// Init ROS
ros::init(argc, argv, "tp4");
// ROS variables/objects
ros::NodeHandle nh; // Node handle
ros::Publisher vel_pub; // Velocity commands publisher
geometry_msgs::Twist vel_cmd; // Velocity commands
std::cout << "Random navigation with obstacle avoidance and map generation\n"
<< "---------------------------" << std::endl;
// Get parameters
ros::NodeHandle n_private("~");
n_private.param("min_front_dist", min_front_dist, 1.0);
n_private.param("stop_front_dist", stop_front_dist, 0.6);
/// Setup subscribers
// Odometry
ros::Subscriber sub_odom = nh.subscribe("/robot_0/odom", 1, odomCallback);
// Laser scans
ros::Subscriber sub_laser = nh.subscribe("/robot_0/base_scan", 1, laserCallback);
// Point clouds
ros::Subscriber sub_pcl = nh.subscribe("cloud_filtered", 1, pointCloudCallback);
/// Setup publisher
vel_pub = nh.advertise<geometry_msgs::Twist>("/robot_0/cmd_vel", 1);
// Infinite loop
ros::Rate cycle(10.0); // Rate when no key is being pressed
while(ros::ok())
{
// Get data from the robot and print it if available
ros::spinOnce();
// Only change navigation controls if laser was updated
if( laser_updated == false )
continue;
// show pose estimated from odometry
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3)
<< "Robot estimated pose = "
<< robot_pose.x << " [m], " << robot_pose.y << " [m], "
<< RAD2DEG(robot_pose.theta) << " [º]\n";
// Show estimated velocity
std::cout << "Robot estimated velocity = "
<< true_lin_vel << " [m/s], "
<< RAD2DEG(true_ang_vel) << " [º/s]\n";
// Check for obstacles near the front of the robot
avoid = false;
if( closest_front_obstacle < min_front_dist )
{
if( closest_front_obstacle < stop_front_dist )
{
avoid = true;
lin_vel = -0.100;
} else
{
avoid = true;
lin_vel = 0;
}
} else
{
lin_vel = 0.8;
ang_vel = 0;
new_rotation = false;
}
// Rotate to avoid obstacles
if(avoid)
{
if( new_rotation == false )
{
float rnd_point = cvRandReal(&rnggstate );
if( rnd_point >= 0.9 )
{
last_ang_vel = -last_ang_vel;
}
}
ang_vel = last_ang_vel;
new_rotation = true;
}
// Limit maximum velocities
// (not needed here)
// lin_vel = clipValue(lin_vel, -MAX_LIN_VEL, MAX_LIN_VEL);
// ang_vel = clipValue(ang_vel, -MAX_ANG_VEL, MAX_ANG_VEL);
// Show desired velocity
std::cout << "Robot desired velocity = "
<< lin_vel << " [m/s], "
<< RAD2DEG(lin_vel) << " [º/s]" << std::endl;
// Send velocity commands
vel_cmd.angular.z = ang_vel;
vel_cmd.linear.x = lin_vel;
vel_pub.publish(vel_cmd);
// Terminate loop if Escape key is pressed
if( cv::waitKey(10) == 27 )
break;
// Proceed at desired framerate
cycle.sleep();
}
// If we are quitting, stop the robot
vel_cmd.angular.z = 0;
vel_cmd.linear.x = 0;
vel_pub.publish(vel_cmd);
// Store map
cv::imwrite("mapa.png", map);
// Close file
outfile.close();
return 1;
}