restructured ti_comm dir

src/kart_2dnav/launch/sim_autocross_track.launch

@@ -13,13 +13,13 @@
   <param name="/use_sim_time" value="true"/>
 
   <!-- Launch the RViz simulation -->
-  <node pkg="stage_ros_mod_tf" type="stage_ros_mod_tf_node" 
+  <!--<node pkg="stage_ros_mod_tf" type="stage_ros_mod_tf_node" 
         name="stage_ros_mod_tf_node" 
         args="$(find kart_2dnav)/stage/autocross_track.world">
-    <!-- Remap the scan topic to top/scan to match naming used for output
-         scan topic from pointcloud_to_laserscan -->
+   Remap the scan topic to top/scan to match naming used for output
+         scan topic from pointcloud_to_laserscan 
     <remap from="base_scan" to="top/scan"/>
-  </node>
+  </node>-->
 
   <!-- LASER ODOMETRY -->
   <!-- This node is responsible for taking incremental 2D laserscans (/top/scan)
@@ -68,7 +68,7 @@
        Note: This should only be set when running the system with simulated 
              sensor data (rosbags).
   -->
-  <node name="rviz" pkg="rviz" type="rviz" 
-        args="-d $(find kart_2dnav)/params/kart_rviz.rviz" />
+  <!--<node name="rviz" pkg="rviz" type="rviz" 
+        args="-d $(find kart_2dnav)/params/kart_rviz.rviz" />-->
 
 </launch>

src/kart_2dnav/launch/sim_autocross_track_only_goal_setting.launch

@@ -25,7 +25,7 @@
   <param name="/use_sim_time" value="true"/>
 
   <!--  ************** Stage Simulator ***************  -->
-  <node pkg="stage_ros" type="stageros" name="stageros" args="$(find kart_2dnav)/stage/autocross_track.world">
+  <node pkg="stage_ros" type="stageros" name="stageros" args="-g $(find kart_2dnav)/stage/autocross_track.world">
     <!-- Use top/scan to match naming used for output scan topic from pointcloud_to_laserscan -->
     <remap from="base_scan" to="top/scan"/>
   </node>
@@ -74,6 +74,6 @@
   </node>
 
   <!--  **************** Visualisation ****************  -->
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>
+<!--  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>-->
 
 </launch>

src/ti_comm/src/Makefile

@@ -1,20 +1,22 @@
-# Compiler Option
-CC = g++
+WARNING = -Wall -Wshadow --pedantic
+ERROR = -Wvla -Werror
+GCC = g++ -std=c++11 -lstdc++ -lm
+PKG_CONFIG = pkg-config
+CFLAGS = -g $(WARNING) $(ERROR) $(shell $(PKG_CONFIG) --cflags libserialport)
+LIBS = $(shell $(PKG_CONFIG) --libs libserialport)
 
-# Compiler Flags
-CFLAGS = -g -Wall
-TESTFLAGS = -DDEBUG=1
+TESTFLAGS = -DDEBUG=1 -DDEBUG_TX=1 -DDEBUG_RX=1 
+SOURCES = $(wildcard *.cpp)
 
-default: all
+BINARIES = $(SOURCES:.cpp=)
 
-all: amp_serial_jetson.o libserialport-0.1.0/serialport.o libserialport-0.1.0/linux_termios.o
-	$(CC) $(CFLAGS) $(TESTFLAGS) -o amp_serial_jetson amp_serial_jetson.o libserialport-0.1.0/serialport.o libserialport-0.1.0/linux_termios.o -ludev
+%: %.cpp
+	$(GCC) $(CFLAGS) $< $(LIBS) -o $@
 
-amp_serial_jetson.o: amp_serial_jetson.cpp amp_serial_jetson.h util.h amp_err.h
-	$(CC) $(CFLAGS) $(TESTFLAGS) -c amp_serial_jetson.cpp
+debug_%: %.cpp
+	$(GCC) $(CFLAGS) $(TESTFLAGS) $< $(LIBS) -o $@
 
-sample_publisher: sample_publisher.o
-	$(CC) $(CFLAGS) -o sample_publisher sample_publisher.o
+all: $(BINARIES)
 
-sample_publisher.o: sample_publisher.cpp
-	$(CC) $(CFLAGS) -c sample_publisher.cpp
+clean:
+	rm $(BINARIES)

src/ti_comm/src/amp_serial_jetson.cpp

@@ -17,7 +17,7 @@
 #include "geometry_msgs/Twist.h"
 
 // External Libraries
-#include "libserialport/libserialport.h"
+#include <libserialport.h>
 
 // User Defined Libraries / Headers
 #include "amp_err.h"
@@ -65,7 +65,7 @@ int main(int argc, char** argv) {
     config.xon_xoff   =  AMP_SERIAL_CONFIG_XST; 
 
     // Initialize the Serial Port
-    amp_serial_jetson_initialize();
+    amp_serial_jetson_initialize(port);
 
     //amp_serial_jetson_enable_default();
 
@@ -155,7 +155,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) {
  *
  * initializes any given port
  */
- amp_err_code_t amp_serial_jetson_initialize() {
+ amp_err_code_t amp_serial_jetson_initialize(sp_port * _port) {
     // Declare & Initialize Local Variables
 
     #ifdef DEBUG
@@ -173,8 +173,7 @@ void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) {
     // Open the Serial Port based on the Previous Handle
     fprintf(fptr1, "Opening port...\n");
     #endif
-
-    check(sp_open(port, (enum sp_mode)(SP_MODE_READ | SP_MODE_WRITE)), AMP_SERIAL_ERROR_INIT);
+    check(sp_open(port, SP_MODE_READ_WRITE), AMP_SERIAL_ERROR_INIT);
 
     #ifdef DEBUG
     fprintf(fptr1, "Setting configurations...\n");
@@ -612,7 +611,7 @@ void end_program(amp_err_code_t amp_err)
 {
 	/* Free any structures we allocated. */
 	if (&config != NULL)
-		//sp_free_config(&config);
+		sp_free_config(&config);
 	if (port != NULL)
 		sp_free_port(port);
 

src/ti_comm/src/amp_serial_jetson.h

@@ -16,7 +16,7 @@
 #define AMP_SERIAL_START_PKT     0x02                       // start packet byte
 #define AMP_SERIAL_STOP_PKT      0x03                       // end packet byte
 
-#define AMP_SERIAL_CONFIG_BAUD   4800                       // The Configuration Baud Rate
+#define AMP_SERIAL_CONFIG_BAUD   9600                       // The Configuration Baud Rate
 #define AMP_SERIAL_CONFIG_BITS   8                          // The Configuration Bits per Byte
 #define AMP_SERIAL_CONFIG_PARY   SP_PARITY_NONE             // The Configuration Parity Bits
 #define AMP_SERIAL_CONFIG_STOP   1                          // The Configuration Stop Bits
@@ -29,9 +29,9 @@
 
 #define AMP_SERIAL_RC_CTRL_MAX_VEL 10.0f                    // Max Remote Control Velocity in (m/s)
 #define AMP_SERIAL_RC_CTRL_MAX_ANG 20.0f                    // Max Remote Control Steering Angle (rads)
-#define AMP_MAX_VEL 0.4f
+#define AMP_MAX_VEL 10.0f
 #define AMP_MIN_VEL 0.0f
-#define AMP_MAX_ANG 1.4f
+#define AMP_MAX_ANG 5.0f
 #define AMP_MIN_ANG 0.0f
 
 #define PS3_BUTTON_SELECT            0
@@ -113,8 +113,8 @@ typedef enum amp_control_state_t {
 
 // DECLARE PACKET DATA STRUCTURES
 typedef struct amp_serial_pkt_control_t {
-    uint8_t v_angle;                                          // vehicle steering angle
-    uint8_t v_speed;                                          // vehicle speed
+    int v_angle;                                          // vehicle steering angle
+    int v_speed;                                          // vehicle speed
 } amp_serial_pkt_control_t;
 
 typedef struct amp_serial_pkt_dac_t {
@@ -160,38 +160,32 @@ typedef struct amp_serial_pkt_t {
 
 
 // FUNCTION DECLARATIONS --------------------------------------------------
-amp_err_code_t amp_serial_jetson_initialize();
+amp_err_code_t amp_serial_jetson_initialize(sp_port * _port);
 
 void amp_serial_jetson_config_port(sp_port * _port, sp_port_config _config);
 
 void amp_serial_jetson_check_port(sp_port * _port, sp_port_config _config);
 
-//void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg);
+void key_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg);
 
-//void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
+void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
 
 amp_err_code_t amp_serial_jetson_tx_pkt(amp_serial_pkt_t * pkt, int * size);
 
 void amp_serial_jetson_build_packet(amp_serial_pkt_t * pkt, uint8_t * s_data);
 
 amp_err_code_t amp_serial_jetson_tx_byte(uint8_t * s_byte);
 
-int send(amp_serial_pkt_t * pkt);
-
 amp_err_code_t amp_serial_jetson_rx_pkt(amp_serial_pkt_t * pkt, int bytes);
 
 uint8_t amp_serial_jetson_rebuild_packet(amp_serial_pkt_t * pkt, uint8_t * s_buf);
 
 amp_err_code_t amp_serial_jetson_rx_byte(uint8_t * s_byte);
 
-int receive(amp_serial_pkt_t * pkt, int size);
-
 int float_to_int(float max, float min, float num);
 
 void amp_serial_jetson_enable_kart();
 
-void amp_serial_jetson_kill_kart();
-
 void amp_serial_jetson_enable_drive();
 
 void amp_serial_jetson_enable_default();