/////////////////////////////////
// MAIN //
/////////////////////////////////
//----------------------------------------------------------------------------------------
int main(int argc, char **argv)
{
ros::init(argc, argv, "grasp_server");
double spin_frequency=100;
std::string searched_param;
ros::NodeHandle nh_private_("~");
if(nh_private_.searchParam("spin_frequency",searched_param))
nh_private_.getParam(searched_param, spin_frequency);
ICR::GraspServer grasp_server;
ROS_INFO("Grasp server ready");
ros::Rate r(spin_frequency);
while(ros::ok())
{
grasp_server.spin();
r.sleep();
}
return 0;
}
int main(int argc, char** argv){
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::NodeHandle nh_private_("~");
ros::Subscriber sub = n.subscribe("raw_vel", 50, vel_callback);
ros::Subscriber imu_sub = n.subscribe("imu/data", 50, imu_callback);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
double rate = 10.0;
double dt = 0.0;
double x_pos = 0.0;
double y_pos = 0.0;
double theta = 0.0;
ros::Rate r(rate);
while(n.ok()){
ros::spinOnce();
current_time = ros::Time::now();
dt = vel_dt;
//calculate change in time (dt)
double dtt = (current_time - last_time).toSec();
//linear velocity is the linear velocity published from the Teensy board(raw_vel)
double linear_velocity = raw_vel;
//angular velocity is the rotation in Z from imu_filter_madgwick's output
double angular_velocity = imu_z;
//calculate angular displacement θ = ω * t
double delta_theta = angular_velocity * dtt; //radians
//calculate linear displacement in X axis
double delta_x = (linear_velocity * cos(theta)) * dt; //m
//calculate linear displacement in Y axis
double delta_y = (linear_velocity * sin(theta)) * dt; //m
//calculate current position of the robot
//where (x,y) is summation of linear and angular displacement
x_pos += delta_x;
y_pos += delta_y;
theta += delta_theta;
//calculate robot's heading in quaternion angle
//ROS has a function to calculate yaw in quaternion angle
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
//robot's position in x,y, and z
odom_trans.transform.translation.x = x_pos;
odom_trans.transform.translation.y = y_pos;
odom_trans.transform.translation.z = 0.0;
//robot's heading in quaternion
odom_trans.transform.rotation = odom_quat;
odom_trans.header.stamp = current_time;
//publish robot's tf using odom_trans object
odom_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//robot's position in x,y, and z
odom.pose.pose.position.x = x_pos;
odom.pose.pose.position.y = y_pos;
odom.pose.pose.position.z = 0.0;
//robot's heading in quaternion
odom.pose.pose.orientation = odom_quat;
odom.child_frame_id = "base_link";
//linear speed from encoders
odom.twist.twist.linear.x = linear_velocity;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
//angular speed from IMU
odom.twist.twist.angular.z = imu_z;
odom_pub.publish(odom);
last_time = current_time;
r.sleep();
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::NodeHandle nh_private_("~");
//lauch params
int odom_publish_rate = 50;
std::string baselink_frame;
std::string odom_frame;
std::string imu_topic;
std::string vel_topic;
nh_private_.param("odom_publish_rate", odom_publish_rate, 50);
nh_private_.param<std::string>("baselink_frame", baselink_frame, "base_link");
nh_private_.param<std::string>("odom_frame", odom_frame, "odom");
nh_private_.param<std::string>("imu_topic", imu_topic, "imu/data");
nh_private_.param<std::string>("vel_topic", vel_topic, "raw_vel");
//ros::Subscriber sub = n.subscribe("raw_vel", 50, velCallback);
//ros::Subscriber imu_sub = n.subscribe("imu/data", 50, IMUCallback);
//ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
ros::Subscriber sub = n.subscribe(vel_topic, 50, velCallback);
ros::Subscriber imu_sub = n.subscribe(imu_topic, 50, IMUCallback);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>(odom_frame, odom_publish_rate);
tf::TransformBroadcaster odom_broadcaster;
double rate = 10.0;
double x_pos = 0.0;
double y_pos = 0.0;
double theta = 0.0;
ros::Rate r(odom_publish_rate);
while(n.ok()){
ros::spinOnce();
ros::Time current_time = ros::Time::now();
//linear velocity is the linear velocity published from the Teensy board in x axis
double linear_velocity_x = g_vel_x;
//linear velocity is the linear velocity published from the Teensy board in y axis
double linear_velocity_y = g_vel_y;
//angular velocity is the rotation in Z from imu_filter_madgwick's output
double angular_velocity = g_imu_z;
//calculate angular displacement θ = ω * t
double delta_theta = angular_velocity * g_imu_dt; //radians
double delta_x = (linear_velocity_x * cos(theta) - linear_velocity_y * sin(theta)) * g_vel_dt; //m
double delta_y = (linear_velocity_x * sin(theta) + linear_velocity_y * cos(theta)) * g_vel_dt; //m
//calculate current position of the robot
x_pos += delta_x;
y_pos += delta_y;
theta += delta_theta;
//calculate robot's heading in quarternion angle
//ROS has a function to calculate yaw in quaternion angle
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = odom_frame;
odom_trans.child_frame_id = baselink_frame;
//robot's position in x,y, and z
odom_trans.transform.translation.x = x_pos;
odom_trans.transform.translation.y = y_pos;
odom_trans.transform.translation.z = 0.0;
//robot's heading in quaternion
odom_trans.transform.rotation = odom_quat;
odom_trans.header.stamp = current_time;
//publish robot's tf using odom_trans object
odom_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = odom_frame;
//robot's position in x,y, and z
odom.pose.pose.position.x = x_pos;
odom.pose.pose.position.y = y_pos;
odom.pose.pose.position.z = 0.0;
//robot's heading in quaternion
odom.pose.pose.orientation = odom_quat;
odom.child_frame_id = baselink_frame;
//linear speed from encoders
odom.twist.twist.linear.x = linear_velocity_x;
odom.twist.twist.linear.y = linear_velocity_y;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
//angular speed from IMU
odom.twist.twist.angular.z = g_imu_z;
//TODO: include covariance matrix here
//See: https://github.com/chicagoedt/revo_robot/commit/620f3f61ea8ac832e2040fb4f4e5583a15e23e29
odom_pub.publish(odom);
g_last_loop_time = current_time;
r.sleep();
}
}
//start initialize
void VFHPlannerROS::initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros){
if(!initialized_){
ROS_INFO("Initializing VFH");
costmap_ros_ = costmap_ros;
tf_ = tf;
global_frame_ = costmap_ros_->getGlobalFrameID();
robot_base_frame_ = costmap_ros_->getBaseFrameID();
inflation_radius_ = costmap_ros_->getInflationRadius();
ros::NodeHandle nh_private_("~/" + name);
g_plan_pub_ = nh_private_.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = nh_private_.advertise<nav_msgs::Path>("local_plan", 1);
//setto i parametri fissi
m_cell_size = 100; // mm, cell dimension
m_window_diameter = 60; // number of cells
m_sector_angle = 5; // deg, sector angle
//leggo i parametri intercambiabili
if (!nh_private_.getParam ("m_safety_dist_0ms", m_safety_dist_0ms))
m_safety_dist_0ms = 100; // mm, double, safe distance at 0 m/s
if (!nh_private_.getParam ("m_safety_dist_1ms", m_safety_dist_1ms))
m_safety_dist_1ms = 100; // mm, double, safe distance at 1 m/s
if (!nh_private_.getParam ("m_max_speed", m_max_speed))
m_max_speed= 200; // mm/sec, int, max speed
if (!nh_private_.getParam ("m_max_speed_narrow_opening", m_max_speed_narrow_opening))
m_max_speed_narrow_opening= 200; // mm/sec, int, max speed in the narrow opening
if (!nh_private_.getParam ("m_max_speed_wide_opening", m_max_speed_wide_opening))
m_max_speed_wide_opening= 300; // mm/sec, int, max speed in the wide opening
if (!nh_private_.getParam ("m_max_acceleration", m_max_acceleration))
m_max_acceleration = 200; // mm/sec^2, int, max acceleration
if (!nh_private_.getParam ("m_min_turnrate", m_min_turnrate))
m_min_turnrate = 40; // deg/sec, int, min turn rate <--- not used
if (!nh_private_.getParam ("m_max_turnrate_0ms", m_max_turnrate_0ms))
m_max_turnrate_0ms = 40; // deg/sec, int, max turn rate at 0 m/s
if (!nh_private_.getParam ("m_max_turnrate_1ms", m_max_turnrate_1ms))
m_max_turnrate_1ms = 40; // deg/sec, int, max turn rate at 1 m/s
m_min_turn_radius_safety_factor = 1.0; // double ????
if (!nh_private_.getParam ("m_free_space_cutoff_0ms", m_free_space_cutoff_0ms))
m_free_space_cutoff_0ms = 2000000.0; //double, low threshold free space at 0 m/s
if (!nh_private_.getParam ("m_obs_cutoff_0ms", m_obs_cutoff_0ms))
m_obs_cutoff_0ms = 4000000.0; //double, high threshold obstacle at 0 m/s
if (!nh_private_.getParam ("m_free_space_cutoff_1ms", m_free_space_cutoff_1ms))
m_free_space_cutoff_1ms = 2000000.0; //double, low threshold free space at 1 m/s
if (!nh_private_.getParam ("m_obs_cutoff_1ms", m_obs_cutoff_1ms))
m_obs_cutoff_1ms = 4000000.0; //double, high threshold obstacle at 1 m/s
if (!nh_private_.getParam ("m_weight_desired_dir", m_weight_desired_dir))
m_weight_desired_dir = 10.0; //double, weight desired direction
if (!nh_private_.getParam ("m_weight_current_dir", m_weight_current_dir))
m_weight_current_dir = 1.0; //double, weight current direction
if (!nh_private_.getParam ("m_robot_radius", m_robot_radius))
m_robot_radius = 300.0; // robot radius in mm
if (!nh_private_.getParam ("odom_topic_", odom_topic_))
odom_topic_ = "odom";
if (!nh_private_.getParam ("scan_topic_", scan_topic_))
scan_topic_ = "base_scan/scan";
ROS_INFO("topic: %s , %s", scan_topic_.c_str(), odom_topic_.c_str());
nh_private_.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
nh_private_.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);//default 0.10
//inizializzo l'oggetto vfh algorithm
m_vfh = new VFH_Algorithm(m_cell_size, m_window_diameter, m_sector_angle,
m_safety_dist_0ms, m_safety_dist_1ms, m_max_speed,
m_max_speed_narrow_opening, m_max_speed_wide_opening,
m_max_acceleration, m_min_turnrate, m_max_turnrate_0ms,
m_max_turnrate_1ms, m_min_turn_radius_safety_factor,
m_free_space_cutoff_0ms, m_obs_cutoff_0ms, m_free_space_cutoff_1ms,
m_obs_cutoff_1ms, m_weight_desired_dir, m_weight_current_dir);
m_vfh->SetRobotRadius(m_robot_radius);
m_vfh->Init();
ros::NodeHandle gn;
// subscribe to topics
scan_subscriber_ = gn.subscribe(
scan_topic_, 1, &VFHPlannerROS::scanCallback, this);
odom_subscriber_ = gn.subscribe(
odom_topic_, 1, &VFHPlannerROS::odomCallback, this);
initialized_ = true;
}
else
{
ROS_WARN("This planner has already been initialized, doing nothing.");
}
}
