diff --git a/seat_car_controller/include/seat_car_controller.h b/seat_car_controller/include/seat_car_controller.h
index 92388f1fcfe8d108269bded97e40fd43a741609b..432e632d9f79d1d7fe45719d925ca111825e94a7 100644
--- a/seat_car_controller/include/seat_car_controller.h
+++ b/seat_car_controller/include/seat_car_controller.h
@@ -14,6 +14,7 @@
// ROS
#include <ros/node_handle.h>
#include <std_msgs/Int16.h>
+#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
@@ -50,14 +51,12 @@ namespace seat_car_controller
void steering_callback(const std_msgs::Int16::ConstPtr& msg);
ros::Subscriber speed_sub;
void speed_callback(const std_msgs::Int16::ConstPtr& msg);
- ros::Subscriber stop_sub;
- void stop_callback(const std_msgs::Int16::ConstPtr& msg);
ros::Subscriber imu_sub;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
double heading;
ros::Publisher yaw_pub;
- ros::Publisher revolutions_pub;
ros::Publisher twist_pub;
+ ros::Publisher steer_angle_pub;
typedef typename HardwareInterface::ResourceHandleType JointHandle;
@@ -73,10 +72,9 @@ namespace seat_car_controller
double wheel_diameter;
double zero_steer_angle;
std::string steering_topic;
+ std::string steering_fb_topic;
std::string speed_topic;
- std::string stop_topic;
std::string yaw_topic;
- std::string revolutions_topic;
std::string twist_topic;
// joint commands
diff --git a/seat_car_controller/include/seat_car_controller_impl.h b/seat_car_controller/include/seat_car_controller_impl.h
index b50aeb4ffb4826ac273819f3c254c16bdec47e9b..39e32ee371fe863a69766121facb3e257f01ce38 100644
--- a/seat_car_controller/include/seat_car_controller_impl.h
+++ b/seat_car_controller/include/seat_car_controller_impl.h
@@ -211,26 +211,22 @@ namespace seat_car_controller
controller_nh_.getParam("zero_steer_angle", this->zero_steer_angle);
ROS_DEBUG_STREAM_NAMED(name_, "Zero steer angle set to: " << this->zero_steer_angle);
- this->steering_topic="/manual_control/steering";
+ this->steering_topic="/steering";
controller_nh_.getParam("steering_topic", this->steering_topic);
ROS_DEBUG_STREAM_NAMED(name_, "Steering topic set to: " << this->steering_topic);
+ this->steering_fb_topic="/steering_angle";
+ controller_nh_.getParam("steering_fb_topic", this->steering_fb_topic);
+ ROS_DEBUG_STREAM_NAMED(name_, "Steering feedback topic set to: " << this->steering_fb_topic);
+
this->speed_topic="/manual_control/speed";
controller_nh_.getParam("speed_topic", this->speed_topic);
ROS_DEBUG_STREAM_NAMED(name_, "Speed topic set to: " << this->speed_topic);
- this->stop_topic="/manual_control/stop";
- controller_nh_.getParam("stop_topic", this->stop_topic);
- ROS_DEBUG_STREAM_NAMED(name_, "Stop topic set to: " << this->stop_topic);
-
this->yaw_topic="/model_car/yaw";
controller_nh_.getParam("yaw_topic", this->yaw_topic);
ROS_DEBUG_STREAM_NAMED(name_, "Yaw topic set to: " << this->yaw_topic);
- this->revolutions_topic="/model_car/revolutions";
- controller_nh_.getParam("revolutions_topic", this->revolutions_topic);
- ROS_DEBUG_STREAM_NAMED(name_, "Revolutions topic set to: " << this->revolutions_topic);
-
this->twist_topic="/model_car/twist";
controller_nh_.getParam("twist_topic", this->twist_topic);
ROS_DEBUG_STREAM_NAMED(name_, "Twist topic set to: " << this->twist_topic);
@@ -250,7 +246,6 @@ namespace seat_car_controller
// ros communications
this->steering_sub = root_nh.subscribe(this->steering_topic, 1, &SeatCarController<HardwareInterface>::steering_callback,this);
this->speed_sub = root_nh.subscribe(this->speed_topic, 1, &SeatCarController<HardwareInterface>::speed_callback,this);
- this->stop_sub = root_nh.subscribe(this->stop_topic, 1, &SeatCarController<HardwareInterface>::stop_callback,this);
this->imu_sub = root_nh.subscribe("/imu/data", 1, &SeatCarController<HardwareInterface>::imu_callback,this);
ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
@@ -258,8 +253,8 @@ namespace seat_car_controller
"\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
this->yaw_pub = root_nh.advertise<std_msgs::Float32>(this->yaw_topic,1);
- this->revolutions_pub = root_nh.advertise<std_msgs::Float32>(this->revolutions_topic,1);
this->twist_pub = root_nh.advertise<geometry_msgs::Twist>(this->twist_topic,1);
+ this->steer_angle_pub = root_nh.advertise<std_msgs::Int16>(this->steering_fb_topic,1);
this->last_cmd_drive=0.0;
this->last_cmd_steer=0.0;
@@ -271,13 +266,13 @@ namespace seat_car_controller
template <class HardwareInterface>
void SeatCarController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
{
- double error,command,error_v;
+ double error,command,error_v,servo_angle;
double steer_l_pos,steer_r_pos,drive_r_l_pos,drive_r_r_pos,drive_f_l_pos,drive_f_r_pos;
double steer_l_vel,steer_r_vel,drive_r_l_vel,drive_r_r_vel,drive_f_l_vel,drive_f_r_vel;
static double yaw=0.0;
static double last_steer_angle=0.0;
- std_msgs::Float32 revolutions_msg;
geometry_msgs::Twist twist_msg;
+ std_msgs::Int16 steer_angle_msg;
steer_l_pos=this->joints_[0].getPosition();
steer_r_pos=this->joints_[1].getPosition();
@@ -351,15 +346,15 @@ namespace seat_car_controller
command=pids_[5]->computeCommand(error,period);
this->joints_[5].setCommand(command);
- // compute and publish yaw, revolutions and twist
- revolutions_msg.data=-(((drive_r_l_pos-drive_r_r_pos)/2.0)/6.0);
- revolutions_msg.data/=(2.0*3.14159);
- this->revolutions_pub.publish(revolutions_msg);
-
+ // compute and publish yaw and twist
twist_msg.linear.x=-((drive_r_l_vel-drive_r_r_vel)*5.5)/2.0;
twist_msg.linear.y=0.0;
twist_msg.linear.z=0.0;
this->twist_pub.publish(twist_msg);
+
+ servo_angle=-4.0*steer_angle+3.14159/2.0;
+ steer_angle_msg.data=180+(unsigned short int)((servo_angle*180.0)/3.14159);
+ this->steer_angle_pub.publish(steer_angle_msg);
}
template <class HardwareInterface>
@@ -369,7 +364,7 @@ namespace seat_car_controller
double car_angle;
double radius;
- car_angle=-(this->steer_coeff_a*pow(servo_angle,2.0)+this->steer_coeff_b*servo_angle+this->steer_coeff_c);
+ car_angle=-((servo_angle-3.14159/2.0)/4.0);
if(fabs(car_angle)>0.001)
radius=fabs(this->axel_distance/tan(car_angle));
else
@@ -408,7 +403,7 @@ namespace seat_car_controller
radius=std::numeric_limits<double>::max();
/* convert input data [-1000 <-> 1000] */
- motor_voltage=(-msg->data/4.0)*5.0/255.0;// cmd*pwm_max_voltage/pwm_number of steps
+ motor_voltage=(msg->data/4.0)*5.0/255.0;// cmd*pwm_max_voltage/pwm_number of steps
motor_speed=motor_voltage*1000.0; // motor_voltage*speed_controller conversion factor (1V <-> 1000 rev/min)
wheel_speed=motor_speed/5.5; // motor_speed / gear_ratio of the car
linear_speed=wheel_speed*3.13159*this->wheel_diameter/60.0; // linear speed in m/s
@@ -434,23 +429,13 @@ namespace seat_car_controller
this->last_cmd_drive=linear_speed;
}
- template <class HardwareInterface>
- void SeatCarController<HardwareInterface>::stop_callback(const std_msgs::Int16::ConstPtr& msg)
- {
- // right inner
- this->rear_right_cmd=0.0;
- // left outer
- this->rear_left_cmd=0.0;
- this->last_cmd_drive=0.0;
- }
-
template <class HardwareInterface>
void SeatCarController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
std_msgs::Float32 yaw_msg;
this->heading=tf::getYaw(msg->orientation);
- yaw_msg.data=this->heading*180.0/3.14159;
+ yaw_msg.data=this->heading;
this->yaw_pub.publish(yaw_msg);
}