car controller

helps forward commands and has a failsafe

ros_ws/dev.launch

@@ -7,6 +7,15 @@
       pkg="auto_race_pg"
       type="remote_control"
       name="remote_control"
+      output="log" >
+    </node>
+
+    <!-- car control node -->
+    <node
+      respawn="true"
+      pkg="auto_race_pg"
+      type="car_control"
+      name="car_control"
       output="screen" >
     </node>
 

ros_ws/src/auto_race_pg/CMakeLists.txt

@@ -130,7 +130,12 @@ include_directories(
 # target_link_libraries(listener ${catkin_LIBRARIES})
 # add_dependencies(listener auto_race_pg_generate_messages_cpp)
 
-# remote controll node 
+# car control node 
+add_executable(car_control src/car_control.cpp)
+target_link_libraries(car_control ${catkin_LIBRARIES})
+add_dependencies(car_control auto_race_pg_generate_messages_cpp)
+
+# remote control node 
 add_executable(remote_control src/remote_control.cpp)
 target_link_libraries(remote_control ${catkin_LIBRARIES})
 add_dependencies(remote_control auto_race_pg_generate_messages_cpp)

ros_ws/src/auto_race_pg/src/car_control.cpp

@@ -0,0 +1,89 @@
+#include <ros/ros.h>
+
+#include <std_msgs/Float64.h>
+
+#define TOPIC_FOCBOX_SPEED "/commands/motor/speed"
+#define TOPIC_FOCBOX_ANGLE "/commands/servo/position"
+
+#define TOPIC_SPEED "/set/speed"
+#define TOPIC_ANGLE "/set/position"
+
+#define MAX_SPEED 5000
+#define MAX_ANGLE 0.8
+
+class CarControl
+{
+    public:
+    CarControl();
+
+    private:
+    ros::NodeHandle nh_;
+
+    ros::Subscriber  in_speed;
+    ros::Subscriber  in_angle;
+
+    void speed_callback(const std_msgs::Float64::ConstPtr& speed);
+    void angle_callback(const std_msgs::Float64::ConstPtr& angle);
+
+    ros::Publisher  out_speed;
+    ros::Publisher  out_angle;
+
+    void adjustSpeed(double speed);
+    void adjustAngle(double angle);
+
+    bool run;
+
+};
+
+CarControl::CarControl()
+    : run(false)
+{
+    out_speed = nh_.advertise< std_msgs::Float64 >(TOPIC_FOCBOX_SPEED, 1);
+    out_angle = nh_.advertise< std_msgs::Float64 >(TOPIC_FOCBOX_ANGLE, 1);
+
+    in_speed =
+        nh_.subscribe< std_msgs::Float64 >(TOPIC_SPEED, 1, &CarControl::speed_callback, this);
+    in_angle =
+        nh_.subscribe< std_msgs::Float64 >(TOPIC_ANGLE, 1, &CarControl::angle_callback, this);
+}
+
+void CarControl::speed_callback(const std_msgs::Float64::ConstPtr& speed) {
+    adjustSpeed(speed->data);
+}
+
+void CarControl::angle_callback(const std_msgs::Float64::ConstPtr& angle) {
+    adjustAngle(angle->data);
+}
+
+
+void CarControl::adjustSpeed(double speed) {
+    if(!run) {
+        std::cout << "fail safe" << std::endl;
+    }
+    else {
+        std::cout << "speed: " << speed << std::endl;
+        std_msgs::Float64 msg;
+        msg.data = speed * MAX_SPEED;
+        out_speed.publish(msg);
+    }
+}
+
+void CarControl::adjustAngle(double angle) {
+    if(!run) {
+        std::cout << "fail safe" << std::endl;
+    }
+    else {
+        std::cout << "angle: " << angle << std::endl;
+        std_msgs::Float64 msg;
+        msg.data = (-angle * MAX_ANGLE + 1) / 2;
+        out_angle.publish(msg);
+    }
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "car_control");
+    CarControl car_control;
+
+    ros::spin();
+}

ros_ws/src/auto_race_pg/src/remote_control.cpp

@@ -1,6 +1,5 @@
 #include <ros/ros.h>
 
-#include <geometry_msgs/Twist.h>
 #include <std_msgs/Float64.h>
 #include <sensor_msgs/Joy.h>
 #include <termios.h>
@@ -10,8 +9,8 @@
 #define MAX_SPEED 5000
 #define MAX_ANGLE 0.8
 
-#define TOPIC_SPEED "/commands/motor/speed"
-#define TOPIC_ANGLE "/commands/servo/position"
+#define TOPIC_SPEED "/set/speed"
+#define TOPIC_ANGLE "/set/position"
 
 #define KEYCODE_W 119
 #define KEYCODE_A 97
@@ -143,12 +142,12 @@ int main(int argc, char** argv)
     ros::init(argc, argv, "remove_control");
     RemoteControl remote_control;
 
-    //std::cout << "listining to keyboard and controller" << std::endl;
-    //std::cout << "====================================" << std::endl;
+    std::cout << "listining to keyboard and controller" << std::endl;
+    std::cout << "====================================" << std::endl;
 
-    //signal(SIGINT, quit);
-    //remote_control.keyLoop();
-    //return 0;
+    signal(SIGINT, quit);
+    remote_control.keyLoop();
+    return 0;
 
-    ros::spin();
+    //ros::spin();
 }