36 #include <sensor_msgs/JointState.h> 37 #include <sensor_msgs/Imu.h> 38 #include <geometry_msgs/Twist.h> 39 #include <std_msgs/Float64.h> 41 #include <nav_msgs/Odometry.h> 42 #include <robotnik_msgs/set_mode.h> 43 #include <robotnik_msgs/get_mode.h> 44 #include <robotnik_msgs/set_odometry.h> 45 #include <robotnik_msgs/ptz.h> 49 #include "diagnostic_msgs/DiagnosticStatus.h" 56 #define PI 3.1415926535 57 #define SUMMIT_XL_MIN_COMMAND_REC_FREQ 5.0 58 #define SUMMIT_XL_MAX_COMMAND_REC_FREQ 150.0 60 #define SKID_STEERING 1 61 #define MECANUM_STEERING 2 63 #define SUMMIT_XL_WHEEL_DIAMETER 0.25 // Default wheel diameter 64 #define SUMMIT_XL_D_TRACKS_M 1.0 // default equivalent W distance (difference is due to slippage of skid steering) 65 #define SUMMIT_XL_WHEELBASE 0.446 // default real L distance forward to rear axis 66 #define SUMMIT_XL_TRACKWIDTH 0.408 // default real W distance from left wheels to right wheels 86 std::string robot_model_;
118 std::string frw_vel_topic_;
119 std::string flw_vel_topic_;
120 std::string brw_vel_topic_;
121 std::string blw_vel_topic_;
124 std::string joint_front_right_wheel;
125 std::string joint_front_left_wheel;
126 std::string joint_back_left_wheel;
127 std::string joint_back_right_wheel;
130 std::string frw_pos_topic_;
131 std::string flw_pos_topic_;
132 std::string brw_pos_topic_;
133 std::string blw_pos_topic_;
136 std::string joint_front_right_steer;
137 std::string joint_front_left_steer;
138 std::string joint_back_left_steer;
139 std::string joint_back_right_steer;
142 std::string scissor_pos_topic_;
145 std::string joint_camera_pan;
146 std::string joint_camera_tilt;
149 std::string pan_pos_topic_;
150 std::string tilt_pos_topic_;
153 std::string scissor_prismatic_joint;
156 int kinematic_modes_;
157 int active_kinematic_mode_;
160 int frw_vel_, flw_vel_, blw_vel_, brw_vel_;
161 int frw_pos_, flw_pos_, blw_pos_, brw_pos_;
163 int pan_pos_, tilt_pos_;
167 double linearSpeedXMps_;
168 double linearSpeedYMps_;
169 double angularSpeedRads_;
172 double robot_pose_px_;
173 double robot_pose_py_;
174 double robot_pose_pa_;
175 double robot_pose_vx_;
176 double robot_pose_vy_;
179 sensor_msgs::JointState joint_state_;
182 geometry_msgs::Twist base_vel_msg_;
190 double pos_ref_tilt_;
196 double summit_xl_wheel_diameter_;
197 double summit_xl_d_tracks_m_;
198 double summit_xl_wheelbase_;
199 double summit_xl_trackwidth_;
210 double orientation_x_;
211 double orientation_y_;
212 double orientation_z_;
213 double orientation_w_;
216 bool publish_odom_tf_;
231 node_handle_(h), private_node_handle_(
"~"),
233 freq_diag_(
diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05) ),
238 ROS_INFO(
"summit_xl_robot_control_node - Init ");
242 kinematic_modes_ = 1;
244 ros::NodeHandle summit_xl_robot_control_node_handle(node_handle_,
"summit_xl_robot_control");
247 if (!private_node_handle_.
getParam(
"model", robot_model_)) {
251 else ROS_INFO(
"Robot Model : %s", robot_model_.c_str());
254 private_node_handle_.
param<std::string>(
"frw_vel_topic", frw_vel_topic_,
"/summit_xl/joint_frw_velocity_controller/command");
255 private_node_handle_.
param<std::string>(
"flw_vel_topic", flw_vel_topic_,
"/summit_xl/joint_flw_velocity_controller/command");
256 private_node_handle_.
param<std::string>(
"blw_vel_topic", blw_vel_topic_,
"/summit_xl/joint_blw_velocity_controller/command");
257 private_node_handle_.
param<std::string>(
"brw_vel_topic", brw_vel_topic_,
"/summit_xl/joint_brw_velocity_controller/command");
260 private_node_handle_.
param<std::string>(
"joint_front_right_wheel", joint_front_right_wheel,
"joint_front_right_wheel");
261 private_node_handle_.
param<std::string>(
"joint_front_left_wheel", joint_front_left_wheel,
"joint_front_left_wheel");
262 private_node_handle_.
param<std::string>(
"joint_back_left_wheel", joint_back_left_wheel,
"joint_back_left_wheel");
263 private_node_handle_.
param<std::string>(
"joint_back_right_wheel", joint_back_right_wheel,
"joint_back_right_wheel");
266 if ((robot_model_==
"summit_xl_omni") || (robot_model_==
"x_wam")) {
267 kinematic_modes_ = 2;
268 private_node_handle_.
param<std::string>(
"frw_pos_topic", frw_pos_topic_,
"/summit_xl/joint_frw_velocity_controller/command");
269 private_node_handle_.
param<std::string>(
"flw_pos_topic", flw_pos_topic_,
"/summit_xl/joint_flw_velocity_controller/command");
270 private_node_handle_.
param<std::string>(
"blw_pos_topic", blw_pos_topic_,
"/summit_xl/joint_blw_velocity_controller/command");
271 private_node_handle_.
param<std::string>(
"brw_pos_topic", brw_pos_topic_,
"/summit_xl/joint_brw_velocity_controller/command");
273 private_node_handle_.
param<std::string>(
"joint_front_right_steer", joint_front_right_steer,
"joint_front_right_steer");
274 private_node_handle_.
param<std::string>(
"joint_front_left_steer", joint_front_left_steer,
"joint_front_left_steer");
275 private_node_handle_.
param<std::string>(
"joint_back_left_steer", joint_back_left_steer,
"joint_back_left_steer");
276 private_node_handle_.
param<std::string>(
"joint_back_right_steer", joint_back_right_steer,
"joint_back_right_steer");
278 if (!private_node_handle_.
getParam(
"summit_xl_wheelbase", summit_xl_wheelbase_))
280 if (!private_node_handle_.
getParam(
"summit_xl_trackwidth", summit_xl_trackwidth_))
284 if (robot_model_==
"x_wam") {
285 private_node_handle_.
param<std::string>(
"scissor_pos_topic", scissor_pos_topic_,
"/summit_xl/joint_scissor_position_controller/command");
286 private_node_handle_.
param<std::string>(
"scissor_prismatic_joint", scissor_prismatic_joint,
"scissor_prismatic_joint");
291 private_node_handle_.
param<std::string>(
"pan_pos_topic", pan_pos_topic_,
"/summit_xl/joint_pan_position_controller/command");
292 private_node_handle_.
param<std::string>(
"tilt_pos_topic", tilt_pos_topic_,
"/summit_xl/joint_tilt_position_controller/command");
293 private_node_handle_.
param<std::string>(
"joint_camera_pan", joint_camera_pan,
"joint_camera_pan");
294 private_node_handle_.
param<std::string>(
"joint_camera_tilt", joint_camera_tilt,
"joint_camera_tilt");
297 if (!private_node_handle_.
getParam(
"summit_xl_wheel_diameter", summit_xl_wheel_diameter_))
299 if (!private_node_handle_.
getParam(
"summit_xl_d_tracks_m", summit_xl_d_tracks_m_))
301 ROS_INFO(
"summit_xl_wheel_diameter_ = %5.2f", summit_xl_wheel_diameter_);
302 ROS_INFO(
"summit_xl_d_tracks_m_ = %5.2f", summit_xl_d_tracks_m_);
304 private_node_handle_.
param(
"publish_odom_tf", publish_odom_tf_,
true);
305 if (publish_odom_tf_)
ROS_INFO(
"PUBLISHING odom->base_footprin tf");
306 else ROS_INFO(
"NOT PUBLISHING odom->base_footprint tf");
309 linearSpeedXMps_ = 0.0;
310 linearSpeedYMps_ = 0.0;
311 angularSpeedRads_ = 0.0;
314 robot_pose_px_ = 0.0;
315 robot_pose_py_ = 0.0;
316 robot_pose_pa_ = 0.0;
317 robot_pose_vx_ = 0.0;
318 robot_pose_vy_ = 0.0;
329 ang_vel_x_ = 0.0; ang_vel_y_ = 0.0; ang_vel_z_ = 0.0;
330 lin_acc_x_ = 0.0; lin_acc_y_ = 0.0; lin_acc_z_ = 0.0;
331 orientation_x_ = 0.0; orientation_y_ = 0.0; orientation_z_ = 0.0; orientation_w_ = 0.0;
348 ref_vel_frw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( frw_vel_topic_, 50);
349 ref_vel_flw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( flw_vel_topic_, 50);
350 ref_vel_blw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( blw_vel_topic_, 50);
351 ref_vel_brw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( brw_vel_topic_, 50);
353 if ((robot_model_==
"summit_xl_omni")||(robot_model_==
"x_wam")) {
354 ref_pos_frw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( frw_pos_topic_, 50);
355 ref_pos_flw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( flw_pos_topic_, 50);
356 ref_pos_blw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( blw_pos_topic_, 50);
357 ref_pos_brw_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( brw_pos_topic_, 50);
359 if (robot_model_==
"x_wam")
360 ref_pos_scissor_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( scissor_pos_topic_, 50);
363 ref_pos_pan_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( pan_pos_topic_, 50);
364 ref_pos_tilt_ = summit_xl_robot_control_node_handle.
advertise<std_msgs::Float64>( tilt_pos_topic_, 50);
375 odom_pub_ = summit_xl_robot_control_node_handle.
advertise<nav_msgs::Odometry>(
"/summit_xl/odom", 1000);
378 diagnostic_.
setHardwareID(
"summit_xl_robot_control - simulation");
379 diagnostic_.
add( freq_diag_ );
380 diagnostic_.
add( command_freq_ );
388 subs_command_freq->
addTask(&command_freq_);
398 ROS_INFO(
"SummitXLControllerClass::starting");
406 vector<string> joint_names = joint_state_.name;
407 frw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_right_wheel)) - joint_names.begin();
408 flw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_left_wheel)) - joint_names.begin();
409 blw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_left_wheel)) - joint_names.begin();
410 brw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_right_wheel)) - joint_names.begin();
411 if ((robot_model_==
"summit_xl_omni")||(robot_model_==
"x_wam")) {
412 frw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_right_steer)) - joint_names.begin();
413 flw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_left_steer)) - joint_names.begin();
414 blw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_left_steer)) - joint_names.begin();
415 brw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_right_steer)) - joint_names.begin();
417 if (robot_model_==
"x_wam") {
418 scissor_pos_ = find (joint_names.begin(),joint_names.end(), string(scissor_prismatic_joint)) - joint_names.begin();
437 double dUl = v_ref_x_ - 0.5 * summit_xl_d_tracks_m_ * w_ref_;
439 double dUr = v_ref_x_ + 0.5 * summit_xl_d_tracks_m_ * w_ref_;
447 std_msgs::Float64 frw_ref_msg;
448 std_msgs::Float64 flw_ref_msg;
449 std_msgs::Float64 blw_ref_msg;
450 std_msgs::Float64 brw_ref_msg;
453 frw_ref_msg.data = -saturation( k1 * dUr, -limit, limit);
454 flw_ref_msg.data = saturation( k1 * dUl, -limit, limit);
455 blw_ref_msg.data = saturation( k1 * dUl, -limit, limit);
456 brw_ref_msg.data = -saturation( k1 * dUr, -limit, limit);
458 ref_vel_frw_.
publish( frw_ref_msg );
459 ref_vel_flw_.
publish( flw_ref_msg );
460 ref_vel_blw_.
publish( blw_ref_msg );
461 ref_vel_brw_.
publish( brw_ref_msg );
640 robot_pose_vx_ = linearSpeedXMps_ * cos(robot_pose_pa_);
641 robot_pose_vy_ = linearSpeedXMps_ * sin(robot_pose_pa_);
648 double v1, v2, v3, v4;
649 v1 = joint_state_.velocity[frw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
650 v2 = joint_state_.velocity[flw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
651 v3 = joint_state_.velocity[blw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
652 v4 = joint_state_.velocity[brw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
653 double a1, a2, a3, a4;
654 a1 = radnorm2( joint_state_.position[frw_pos_] );
655 a2 = radnorm2( joint_state_.position[flw_pos_] );
656 a3 = radnorm2( joint_state_.position[blw_pos_] );
657 a4 = radnorm2( joint_state_.position[brw_pos_] );
658 double v1x = -v1 * cos( a1 );
double v1y = -v1 * sin( a1 );
659 double v2x = v2 * cos( a2 );
double v2y = v2 * sin( a2 );
660 double v3x = v3 * cos( a3 );
double v3y = v3 * sin( a3 );
661 double v4x = -v4 * cos( a4 );
double v4y = -v4 * sin( a4 );
662 double C = (v4y + v1y) / 2.0;
663 double B = (v2x + v1x) / 2.0;
664 double D = (v2y + v3y) / 2.0;
665 double A = (v3x + v4x) / 2.0;
667 robot_pose_vx_ = (A+B) / 2.0;
668 robot_pose_vy_ = (C+D) / 2.0;
672 double fSamplePeriod = 1.0 / desired_freq_;
673 robot_pose_pa_ += ang_vel_z_ * fSamplePeriod;
674 robot_pose_px_ += robot_pose_vx_ * fSamplePeriod;
675 robot_pose_py_ += robot_pose_vy_ * fSamplePeriod;
685 geometry_msgs::TransformStamped odom_trans;
686 odom_trans.header.stamp = current_time;
687 odom_trans.header.frame_id =
"odom";
688 odom_trans.child_frame_id =
"base_footprint";
690 odom_trans.transform.translation.x = robot_pose_px_;
691 odom_trans.transform.translation.y = robot_pose_py_;
692 odom_trans.transform.translation.z = 0.0;
693 odom_trans.transform.rotation.x = orientation_x_;
694 odom_trans.transform.rotation.y = orientation_y_;
695 odom_trans.transform.rotation.z = orientation_z_;
696 odom_trans.transform.rotation.w = orientation_w_;
701 if (publish_odom_tf_) odom_broadcaster.
sendTransform(odom_trans);
704 nav_msgs::Odometry odom;
705 odom.header.stamp = current_time;
706 odom.header.frame_id =
"odom";
710 odom.pose.pose.position.x = robot_pose_px_;
711 odom.pose.pose.position.y = robot_pose_py_;
712 odom.pose.pose.position.z = 0.0;
714 odom.pose.pose.orientation.x = orientation_x_;
715 odom.pose.pose.orientation.y = orientation_y_;
716 odom.pose.pose.orientation.z = orientation_z_;
717 odom.pose.pose.orientation.w = orientation_w_;
719 for(
int i = 0; i < 6; i++)
720 odom.pose.covariance[i*6+i] = 0.1;
723 odom.child_frame_id =
"base_footprint";
725 odom.twist.twist.linear.x = robot_pose_vx_;
726 odom.twist.twist.linear.y = robot_pose_vy_;
727 odom.twist.twist.linear.z = 0.0;
729 odom.twist.twist.angular.x = ang_vel_x_;
730 odom.twist.twist.angular.y = ang_vel_y_;
731 odom.twist.twist.angular.z = ang_vel_z_;
733 for(
int i = 0; i < 6; i++)
734 odom.twist.covariance[6*i+i] = 0.1;
753 double diff = (current_time - last_command_time_).toSec();
756 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Topic is not receiving commands");
760 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Topic receiving commands");
768 v_ref_x_ = saturation(cmd_vel.linear.x, -10.0, 10.0);
769 v_ref_y_ = saturation(cmd_vel.linear.y, -10.0, 10.0);
770 w_ref_ = saturation(cmd_vel.angular.z, -10.0, 10.0);
771 v_ref_z_ = saturation(cmd_vel.linear.z, -10.0, 10.0);
776 bool srvCallback_SetMode(robotnik_msgs::set_mode::Request& request, robotnik_msgs::set_mode::Response& response)
783 ROS_INFO(
"SummitXLControllerClass::srvCallback_SetMode request.mode= %d", request.mode);
784 if (request.mode == 1) {
785 active_kinematic_mode_ = request.mode;
788 if (request.mode == 2) {
789 if (kinematic_modes_ == 2) {
790 active_kinematic_mode_ = request.mode;
799 bool srvCallback_GetMode(robotnik_msgs::get_mode::Request& request, robotnik_msgs::get_mode::Response& response)
801 response.mode = active_kinematic_mode_;
831 subs_command_freq->
tick();
833 base_vel_msg_ = *msg;
834 this->setCommand(base_vel_msg_);
840 pos_ref_pan_ += msg->pan / 180.0 *
PI;
841 pos_ref_tilt_ += msg->tilt / 180.0 *
PI;
847 orientation_x_ = imu_msg.orientation.x;
848 orientation_y_ = imu_msg.orientation.y;
849 orientation_z_ = imu_msg.orientation.z;
850 orientation_w_ = imu_msg.orientation.w;
852 ang_vel_x_ = imu_msg.angular_velocity.x;
853 ang_vel_y_ = imu_msg.angular_velocity.y;
854 ang_vel_z_ = imu_msg.angular_velocity.z;
856 lin_acc_x_ = imu_msg.linear_acceleration.x;
857 lin_acc_y_ = imu_msg.linear_acceleration.y;
858 lin_acc_z_ = imu_msg.linear_acceleration.z;
870 while (value >
PI) value -=
PI;
871 while (value < -
PI) value +=
PI;
877 while (value > 2.0*
PI) value -= 2.0*
PI;
878 while (value < -2.0*
PI) value += 2.0*
PI;
884 ROS_INFO(
"summit_xl_robot_control::spin()");
908 ROS_INFO(
"summit_xl_robot_control::spin() - end");
914 int main(
int argc,
char** argv)
916 ros::init(argc, argv,
"summit_xl_robot_control");
SummitXLControllerClass(ros::NodeHandle h)
double radnorm(double value)
void publish(const boost::shared_ptr< M > &message) const
void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat)
void summary(unsigned char lvl, const std::string s)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define SUMMIT_XL_WHEEL_DIAMETER
#define SUMMIT_XL_WHEELBASE
void setHardwareID(const std::string &hwid)
void jointStateCallback(const sensor_msgs::JointStateConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void imuCallback(const sensor_msgs::Imu &imu_msg)
void commandCallback(const geometry_msgs::TwistConstPtr &msg)
void add(const std::string &name, TaskFunction f)
int starting()
Controller startup in realtime.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
double saturation(double u, double min, double max)
bool srvCallback_SetOdometry(robotnik_msgs::set_odometry::Request &request, robotnik_msgs::set_odometry::Response &response)
bool srvCallback_SetMode(robotnik_msgs::set_mode::Request &request, robotnik_msgs::set_mode::Response &response)
double radnorm2(double value)
void command_ptzCallback(const robotnik_msgs::ptzConstPtr &msg)
#define SUMMIT_XL_MIN_COMMAND_REC_FREQ
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void UpdateControl()
Controller update loop.
bool srvCallback_GetMode(robotnik_msgs::get_mode::Request &request, robotnik_msgs::get_mode::Response &response)
ROSCPP_DECL bool isShuttingDown()
void setCommand(const geometry_msgs::Twist &cmd_vel)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void stopping()
Controller stopping.
#define SUMMIT_XL_TRACKWIDTH
#define SUMMIT_XL_MAX_COMMAND_REC_FREQ
bool getParam(const std::string &key, std::string &s) const
int main(int argc, char **argv)
#define SUMMIT_XL_D_TRACKS_M
ROSCPP_DECL void spinOnce()
void addTask(DiagnosticTask *t)