Merge pull request #5 from cmu-mars/develop

Develop
cmu-mars · May 11, 2018 · 165e557 · 165e557
2 parents 709d8ce + bbc0d56
commit 165e557
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 1 deletion.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -52,6 +52,11 @@ execute_process(COMMAND
 
 catkin_python_setup()
 
+add_message_files(
+  FILES
+  BrassBump.msg
+)
+
 add_service_files(
   FILES
   ToggleHeadlamp.srv

diff --git a/include/kobuki_gazebo_plugins/gazebo_ros_kobuki.h b/include/kobuki_gazebo_plugins/gazebo_ros_kobuki.h
@@ -62,6 +62,7 @@
 #include <kobuki_msgs/MotorPower.h>
 #include <kobuki_msgs/CliffEvent.h>
 #include <kobuki_msgs/BumperEvent.h>
+#include <brass_gazebo_plugins/BrassBump.h>
 
 namespace gazebo
 {
@@ -182,6 +183,8 @@ class GazeboRosKobuki : public ModelPlugin
   ros::Publisher odom_pub_;
   /// ROS message for odometry data
   nav_msgs::Odometry odom_;
+
+  brass_gazebo_plugins::BrassBump brass_bump_;
   /// Flag for (not) publish tf transform for odom -> robot
   bool publish_tf_;
   /// TF transform publisher for the odom frame
@@ -237,6 +240,8 @@ class GazeboRosKobuki : public ModelPlugin
   /// ROS subscriber for reseting the odometry data
   ros::Subscriber odom_reset_sub_;
 
+  ros::Publisher bumper_event_vel_pub_;
+
 
 
 };

diff --git a/msg/BrassBump.msg b/msg/BrassBump.msg
@@ -0,0 +1,11 @@
+uint8 LEFT=0
+uint8 CENTER=1
+uint8 RIGHT=1
+
+uint8 RELEASED=0
+uint8 PRESSED=0
+
+uint8 bumper
+uint8 state
+float64 velocity
+
diff --git a/src/gazebo_ros_kobuki_loads.cpp b/src/gazebo_ros_kobuki_loads.cpp
@@ -351,6 +351,10 @@ void GazeboRosKobuki::setupRosApi(std::string& model_name)
   bumper_event_pub_ = gazebo_ros_->node()->advertise<kobuki_msgs::BumperEvent>(bumper_topic, 1);
   ROS_INFO("%s: Advertise Bumper[%s]!", gazebo_ros_->info(), bumper_topic.c_str());
 
+  std::string bumper_vel_topic = base_prefix + "/events/brass_bump";
+  bumper_event_vel_pub_ = gazebo_ros_->node()->advertise<brass_gazebo_plugins::BrassBump>(bumper_vel_topic, 1);
+  ROS_INFO("%s: Advertise Bumper Bump Velocity[%s]!", gazebo_ros_->info(), bumper_vel_topic.c_str());
+
   // IMU
   std::string imu_topic = base_prefix + "/sensors/imu_data";
   imu_pub_ = gazebo_ros_->node()->advertise<sensor_msgs::Imu>(imu_topic, 1);

diff --git a/src/gazebo_ros_kobuki_updates.cpp b/src/gazebo_ros_kobuki_updates.cpp
@@ -306,7 +306,14 @@ void GazeboRosKobuki::updateBumper()
       // ROS_INFO_STREAM("#Contact " << i << ", rel_contact_pos=" << rel_contact_pos);
       // ROS_INFO_STREAM("global_contact_angle=" << global_contact_angle << ", relative_contact_angle=" << relative_contact_angle);
       calculateBumps(relative_contact_angle);
-      calculateBumps(relative_contact_angle-M_PI);  
+      double nrca = relative_contact_angle-M_PI;
+      if (nrca > M_PI) {
+        nrca = nrca - 2*M_PI;
+      }
+      else if (nrca < -M_PI) {
+        nrca = nrca + 2*M_PI;
+      }
+      calculateBumps(nrca);  
     }
 
   }
@@ -318,41 +325,65 @@ void GazeboRosKobuki::updateBumper()
     bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::PRESSED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::LEFT;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
   else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
   {
     bumper_left_was_pressed_ = false;
     bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::RELEASED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::LEFT;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
   if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
   {
     bumper_center_was_pressed_ = true;
     bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::PRESSED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::CENTER;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
   else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
   {
     bumper_center_was_pressed_ = false;
     bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::RELEASED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::CENTER;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
   if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
   {
     bumper_right_was_pressed_ = true;
     bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::PRESSED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::RIGHT;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
   else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
   {
     bumper_right_was_pressed_ = false;
     bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
     bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
     bumper_event_pub_.publish(bumper_event_);
+    brass_bump_.state = brass_gazebo_plugins::BrassBump::RELEASED;
+    brass_bump_.bumper = brass_gazebo_plugins::BrassBump::RIGHT;
+    brass_bump_.velocity = odom_.twist.twist.linear.x;
+    bumper_event_vel_pub_.publish(brass_bump_);
   }
 }
 }