Rearrange package

panther_body/CMakeLists.txt

@@ -5,11 +5,13 @@ project(panther_body)
 # http://wiki.ros.org/rosserial_arduino/Tutorials/CMake
 
 find_package(catkin REQUIRED COMPONENTS 
-    rosserial_arduino 
-    rosserial_client
-    geometry_msgs
-    std_msgs
-    sensor_msgs
+  ros_teensy
+  rosserial_arduino 
+  rosserial_client
+
+  geometry_msgs
+  std_msgs
+  sensor_msgs
 )
 
 catkin_package()

panther_body/firmware/SFR10.cpp

@@ -28,21 +28,58 @@
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+#if defined(ARDUINO) && ARDUINO >= 100
+    #include "Arduino.h"
+#else
+    #include "WProgram.h"
+#endif
+
 #include <Wire.h>
+
 #include "SFR10.hpp"
 
-SFR10::SFR10(int srfAddress, String name)
+void SFR10_connect(sensor_msgs::Range* range_msg)
+{
+  // Waits to make sure everything is powered up before sending or receiving data
+  Wire.begin();
+  // Initialze range message
+  // SFR10 https://www.robot-electronics.co.uk/htm/srf10tech.htm
+  range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND;
+  // range_msg.header.frame_id =  frameid;
+  range_msg->field_of_view = 0.01;
+  range_msg->min_range = 0.03;
+  range_msg->max_range = 0.4;
+}
+
+void SFR10_init(SFR10_t* SFR10, ros::Publisher* pub, sensor_msgs::Range* range_msg, int address, char* name)
 {
-    // I2C address
-    srfAddress_ = srfAddress;
-    name_ = &name;
+  SFR10->pub = pub;
+  SFR10->msg = range_msg;
+  SFR10->address = address;
+  SFR10->name = name;
 }
 
-int SFR10::getRange()
+int SFR10_publish(ros::NodeHandle* nh, SFR10_t* SFR10)
+{
+  // read range sensor
+  int range = SFR10_getRange(SFR10->address);
+  // Publish message
+  SFR10->msg->header.stamp = nh->now();
+  SFR10->msg->range = range;
+  SFR10->pub->publish(SFR10->msg);
+  // return range
+  return range;
+}
+
+/**
+ * @brief SFR10 sonar
+ * reference: https://www.robot-electronics.co.uk/htm/srf10tech.htm
+ */
+int SFR10_getRange(int srfAddress)
 {
   int range = 0;
   // step 1: instruct sensor to read echoes
-  Wire.beginTransmission(srfAddress_); // transmit to device #112 (0x70)
+  Wire.beginTransmission(srfAddress); // transmit to device #112 (0x70)
   // the address specified in the datasheet is 224 (0xE0)
   // but i2c adressing uses the high 7 bits so it's 112
   Wire.write(byte(0x00));             // sets register pointer to the command register (0x00)
@@ -54,11 +91,11 @@ int SFR10::getRange()
   // step 2: wait for readings to happen
   delay(70);                          // datasheet suggests at least 65 milliseconds
   // step 3: instruct sensor to return a particular echo reading
-  Wire.beginTransmission(srfAddress_); // transmit to device #112
+  Wire.beginTransmission(srfAddress); // transmit to device #112
   Wire.write(byte(0x02));             // sets register pointer to echo #1 register (0x02)
   Wire.endTransmission();             // stop transmitting
   // step 4: request reading from sensor
-  Wire.requestFrom(srfAddress_, 2);    // request 2 bytes from slave device #112
+  Wire.requestFrom(srfAddress, 2);    // request 2 bytes from slave device #112
   // step 5: receive reading from sensor
   if (2 <= Wire.available()) {        // if two bytes were received
     range = Wire.read();              // receive high byte (overwrites previous reading)

panther_body/firmware/SFR10.hpp

@@ -28,25 +28,28 @@
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <Arduino.h>
+#include <ros.h>
+ #include <sensor_msgs/Range.h>
 
-class SFR10
-{
-    public:
-        /**
-         * Initialize a SFR10 sensor
-         */
-        SFR10(int srfAddress, String name);
-        /**
-         * Run configuration script
-         */
-        void init();
-        /**
-         * Read the status of the sensor
-         */
-        int getRange();
-
-    private:
-        int srfAddress_;
-        String* name_;
-};
+typedef struct _SFR10 {
+    sensor_msgs::Range* msg;
+    ros::Publisher* pub;
+    int address;
+    char* name;
+} SFR10_t;
+
+/**
+ *
+ */
+void SFR10_connect(sensor_msgs::Range* range_msg);
+
+
+void SFR10_init(SFR10_t* SFR10, ros::Publisher* pub, sensor_msgs::Range* range_msg, int address, char* name);
+/**
+ *
+ */
+int SFR10_publish(ros::NodeHandle* nh, SFR10_t* SFR10);
+/**
+ * Read the status of the sensor
+ */
+int SFR10_getRange(int srfAddress);

panther_body/firmware/body.cpp

@@ -63,7 +63,7 @@
 ros::NodeHandle nh;
 // Initialization sensors
 //SFR10 sensors[SIZE_SFR10] = {SFR10(112, "sfr10_left"), SFR10(113, "sfr10_right"), SFR10(113, "sfr10_rear")};
-SFR10 sensors[SIZE_SFR10] = {SFR10(112, "sfr10_left"), SFR10(113, "sfr10_right")};
+//SFR10 sensors[SIZE_SFR10] = {SFR10(112, "sfr10_left"), SFR10(113, "sfr10_right")};
 
 void TwistMessageCb( const geometry_msgs::Twist& msg) {
   char tmp_buffer[50];
@@ -84,50 +84,59 @@ void EnableMessageCb( const std_msgs::Bool& msg) {
 // Reference Range message
 // http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Range.html
 sensor_msgs::Range range_msg;
+ros::Publisher pub_range_left("sfr10_left", &range_msg);
+ros::Publisher pub_range_right("sfr10_right", &range_msg);
 // Define the twist subscriber
 ros::Subscriber<geometry_msgs::Twist> twist(TWIST_SUBSCRIBER_TOPIC, &TwistMessageCb);
 ros::Subscriber<std_msgs::Bool> enable_status(ENABLE_SUBSCRIBER_TOPIC, &EnableMessageCb);
-ros::Publisher pub_range("range_data", &range_msg);
 
+char frame_left[] = "frame_left";
+char frame_right[] = "frame_right";
+SFR10_t sensors[SIZE_SFR10];
 
 void setup()
 {
+  // Initializtion ROS node
+  nh.initNode();
   // Initialization LED builtin
   pinMode(LED_BUILTIN, OUTPUT);
   // Initialization NeoPixels
   pinMode(LEDS_LEFT, OUTPUT);
   pinMode(LEDS_RIGHT, OUTPUT);
-  // Waits to make sure everything is powered up before sending or receiving data
-  Wire.begin();
-  // Initializtion ROS node
-  nh.initNode();
+  // Initialize SFR10 sensors
+  SFR10_connect(&range_msg);
+  SFR10_init(&sensors[0], &pub_range_left, &range_msg, 112, frame_left);
+  SFR10_init(&sensors[1], &pub_range_right, &range_msg, 113, frame_right);
+
+  // Publish Sonar range
+  nh.advertise(pub_range_left);
+  nh.advertise(pub_range_right);
+
   // Initialization twist and enable subscriber
   nh.subscribe(twist);
   nh.subscribe(enable_status);
-  // Publish Sonar range
-  nh.advertise(pub_range);
-
-
-  // Initialzie range message
-  // SFR10 https://www.robot-electronics.co.uk/htm/srf10tech.htm
-  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
-  // range_msg.header.frame_id =  frameid;
-  range_msg.field_of_view = 0.01;
-  range_msg.min_range = 0.03;
-  range_msg.max_range = 0.4;
 }
 
 bool status = true;
+unsigned long range_timer;
+
+
 
 void loop()
 {
-  digitalWrite(LED_BUILTIN, status);
-  for(int i = 0; i < SIZE_SFR10; ++i)
-  {
-    sensors[i].getRange();
+  // publish the range value every 50 milliseconds
+  //   since it takes that long for the sensor to stabilize
+  if ( (millis() - range_timer) > 50){
+    // Update led status
+    digitalWrite(LED_BUILTIN, status);
+    //range_msg.range = sensors[0].getRange();
+    int left = SFR10_publish(&nh, &sensors[0]);
+    //int right = SFR10_publish(&nh, &sensors[1]);
+    // Update range timer
+    range_timer =  millis();
+    status = !status;
   }
-  status = !status;
-  delay(500);
+  nh.spinOnce();
 }
 // EOF
 

panther_body/launch/body.launch

@@ -37,10 +37,10 @@ EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   <arg name="output"          default="log" />
 
   <!-- Serial controller -->
-  <node pkg="rosserial_python" type="serial_node.py" name="led_controller" args="$(arg port)" output="$(arg output)">
+  <node pkg="rosserial_python" type="serial_node.py" name="body" args="$(arg port)" output="$(arg output)">
     <!-- Load default linear and angular velocity gain -->  
-    <rosparam command="load" file="$(arg config)" />
+    <!-- <rosparam command="load" file="$(arg config)" />-->
     <!-- Remap twist command topic -->
-    <remap from="/cmd_vel" to="$(arg twist)"/>
+    <!--<remap from="/cmd_vel" to="$(arg twist)"/>-->
   </node>
 </launch>

panther_body/launch/demo.launch

@@ -27,8 +27,8 @@ EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 -->
 <launch>
 
-  <!-- Launch Teesny controller -->
-  <include file="$(find panther_led_controller)/launch/led_controller.launch">
+  <!-- Panther body controller -->
+  <include file="$(find panther_body)/launch/body.launch">
     <arg name="output"     value="screen" />
   </include>
 

panther_body/package.xml

@@ -13,8 +13,10 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <depend>ros_teensy</depend>
   <depend>rosserial_arduino</depend>
   <depend>rosserial_client</depend>
+
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>