bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) { const std::string complete_ns = controller_nh.getNamespace(); std::size_t id = complete_ns.find_last_of("/"); name_ = complete_ns.substr(id + 1); // Get joint names from the parameter server std::vector<std::string> left_wheel_names, right_wheel_names; if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or !getWheelNames(controller_nh, "right_wheel", right_wheel_names)) { return false; } if (left_wheel_names.size() != right_wheel_names.size()) { ROS_ERROR_STREAM_NAMED(name_, "#left wheels (" << left_wheel_names.size() << ") != " << "#right wheels (" << right_wheel_names.size() << ")."); return false; } else { wheel_joints_size_ = left_wheel_names.size(); left_wheel_joints_.resize(wheel_joints_size_); right_wheel_joints_.resize(wheel_joints_size_); } // Odometry related: double publish_rate; controller_nh.param("publish_rate", publish_rate, 50.0); ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at " << publish_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); controller_nh.param("open_loop", open_loop_, open_loop_); controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_); ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << "."); controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_); ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by " << wheel_radius_multiplier_ << "."); int velocity_rolling_window_size = 10; controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size); ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << "."); odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size); // Twist command related: controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_); ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s."); controller_nh.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_); ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_?"enabled":"disabled")); controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_); controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_); controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled")); // Velocity and acceleration limits: controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits ); controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits); controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits ); controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity ); controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity ); controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration ); controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration ); controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk ); controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk ); controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits ); controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits); controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits ); controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity ); controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity ); controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration ); controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration ); controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk ); controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk ); // If either parameter is not available, we need to look up the value in the URDF bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_); bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_); if (!setOdomParamsFromUrdf(root_nh, left_wheel_names[0], right_wheel_names[0], lookup_wheel_separation, lookup_wheel_radius)) { return false; } // Regardless of how we got the separation and radius, use them // to set the odometry parameters const double ws = wheel_separation_multiplier_ * wheel_separation_; const double wr = wheel_radius_multiplier_ * wheel_radius_; odometry_.setWheelParams(ws, wr); ROS_INFO_STREAM_NAMED(name_, "Odometry params : wheel separation " << ws << ", wheel radius " << wr); setOdomPubFields(root_nh, controller_nh); // Get the joint object to use in the realtime loop for (int i = 0; i < wheel_joints_size_; ++i) { ROS_INFO_STREAM_NAMED(name_, "Adding left wheel with joint name: " << left_wheel_names[i] << " and right wheel with joint name: " << right_wheel_names[i]); left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]); // throws on failure right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure } sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this); return true; }
bool OmniDriveController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) { const std::string complete_ns = controller_nh.getNamespace(); std::size_t id = complete_ns.find_last_of("/"); name_ = complete_ns.substr(id + 1); // Get joint names from the parameter server if (!getWheelNames(controller_nh, "wheel1", wheel1_name) || !getWheelNames(controller_nh, "wheel2", wheel2_name) || !getWheelNames(controller_nh, "wheel3", wheel3_name)) { return false; } // Odometry related: double publish_rate; controller_nh.param("publish_rate", publish_rate, 20.0); ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at " << publish_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); // Twist command related: controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_); ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s."); controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_); controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled")); // Velocity and acceleration limits: controller_nh.param("linear/x/has_velocity_limits" , limiter_linear_x_.has_velocity_limits , limiter_linear_x_.has_velocity_limits ); controller_nh.param("linear/x/has_acceleration_limits", limiter_linear_x_.has_acceleration_limits, limiter_linear_x_.has_acceleration_limits); controller_nh.param("linear/x/max_velocity" , limiter_linear_x_.max_velocity , limiter_linear_x_.max_velocity ); controller_nh.param("linear/x/min_velocity" , limiter_linear_x_.min_velocity , -limiter_linear_x_.max_velocity ); controller_nh.param("linear/x/max_acceleration" , limiter_linear_x_.max_acceleration , limiter_linear_x_.max_acceleration ); controller_nh.param("linear/x/min_acceleration" , limiter_linear_x_.min_acceleration , -limiter_linear_x_.max_acceleration ); controller_nh.param("linear/y/has_velocity_limits" , limiter_linear_y_.has_velocity_limits , limiter_linear_y_.has_velocity_limits ); controller_nh.param("linear/y/has_acceleration_limits", limiter_linear_y_.has_acceleration_limits, limiter_linear_y_.has_acceleration_limits); controller_nh.param("linear/y/max_velocity" , limiter_linear_y_.max_velocity , limiter_linear_y_.max_velocity ); controller_nh.param("linear/y/min_velocity" , limiter_linear_y_.min_velocity , -limiter_linear_y_.max_velocity ); controller_nh.param("linear/y/max_acceleration" , limiter_linear_y_.max_acceleration , limiter_linear_y_.max_acceleration ); controller_nh.param("linear/y/min_acceleration" , limiter_linear_y_.min_acceleration , -limiter_linear_y_.max_acceleration ); controller_nh.param("angular/z/has_velocity_limits" , limiter_angular_.has_velocity_limits , limiter_angular_.has_velocity_limits ); controller_nh.param("angular/z/has_acceleration_limits", limiter_angular_.has_acceleration_limits, limiter_angular_.has_acceleration_limits); controller_nh.param("angular/z/max_velocity" , limiter_angular_.max_velocity , limiter_angular_.max_velocity ); controller_nh.param("angular/z/min_velocity" , limiter_angular_.min_velocity , -limiter_angular_.max_velocity ); controller_nh.param("angular/z/max_acceleration" , limiter_angular_.max_acceleration , limiter_angular_.max_acceleration ); controller_nh.param("angular/z/min_acceleration" , limiter_angular_.min_acceleration , -limiter_angular_.max_acceleration ); if (!setOdomParamsFromUrdf(root_nh, wheel1_name, wheel2_name, wheel3_name)) return false; setOdomPubFields(root_nh, controller_nh); // Get the joint object to use in the realtime loop wheel1_joint = hw->getHandle(wheel1_name); // throws on failure wheel2_joint = hw->getHandle(wheel2_name); // throws on failure wheel3_joint = hw->getHandle(wheel3_name); //test sub_command_ = controller_nh.subscribe("smooth_cmd_vel", 1, &OmniDriveController::cmdVelCallback, this); //sub_command_ = controller_nh.subscribe("cmd_vel", 1, &OmniDriveController::cmdVelCallback, this); // ROS_INFO("wocao"); return true; }