Permalink
Browse files

Added demo.

  • Loading branch information...
1 parent 8da727c commit 718f0c34f113a7bfe47f27f0bd16c8d6e0ecd32d @oscillare oscillare committed Jul 14, 2012
View
3 ihe_hardware/turtlebot_button/.gitignore
@@ -2,4 +2,5 @@
bin/
build/
msg_gen/
-src/
+src/turtlebot_button/
+*.swp
View
4 ihe_hardware/turtlebot_button/CMakeLists.txt
@@ -19,7 +19,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
rosbuild_genmsg()
#uncomment if you have defined services
-rosbuild_gensrv()
+#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
@@ -28,3 +28,5 @@ rosbuild_gensrv()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
+
+rosbuild_add_executable(turtlebot_button src/turtlebot_button.cpp)
View
16 ihe_hardware/turtlebot_button/README
@@ -6,10 +6,16 @@ Add the header file for Button.msg to Arduino libraries:
-To run:
-
-1) Upload buttons_turtlebot/buttons_turtlebot.ino onto an Arduino. (Currently
+To publish button messages:
+---------------------------
+1) Upload arduino/buttons_turtlebot.ino onto an Arduino. (Currently
not compatiable with Leonardo.)
-2) Run the rosserial client that forwards the message to the rest of ROS.
+2) Run the rosserial client that forwards the message to the rest of ROS:
+
+ rosrun rosserial_python serial_node.py SERIAL_PORT
+
+To run demo:
+------------
+3) Launch turtlebot_button_op node:
- rosrun rosserial_python serial_node.py SERIAL_PORT
+ roslaunch turtlebot_button turtlebot_button.launch
View
18 ...n/buttons_turtlebot/buttons_turtlebot.ino → ...ebot_button/arduino/buttons_turtlebot.ino
@@ -1,10 +1,14 @@
#include <ros.h>
+#include <std_msgs/Bool.h>
#include <turtlebot_button/Buttons.h>
ros::NodeHandle nh;
+std_msgs::Bool estop_msg;
turtlebot_button::Buttons button_msg;
+
ros::Publisher pub_button("buttons", &button_msg);
+ros::Publisher pub_estop("estop", &estop_msg);
const int num_buttons = 5;
@@ -25,6 +29,7 @@ int i;
void setup() {
nh.initNode();
nh.advertise(pub_button);
+ nh.advertise(pub_estop);
pinMode(estop, INPUT);
pinMode(button0, INPUT);
@@ -39,7 +44,7 @@ void setup() {
// Get initial state
// Depressed estop returns 0; otherwise, 1
- button_msg.estop_state = digitalRead(estop);
+ estop_msg.data = digitalRead(estop);
for (i = 0; i < num_buttons; i++) {
// Buttons are normally high.
@@ -81,16 +86,17 @@ void loop() {
}
}
- if (estop_reading != button_msg.estop_state) {
- button_msg.estop_state = estop_reading;
- published = false;
- }
-
// Publish updated button message
if (!published) {
pub_button.publish(&button_msg);
published = true;
}
+ // Publish estop message if state changed
+ if (estop_reading != estop_msg.data) {
+ estop_msg.data = estop_reading;
+ pub_estop.publish(&estop_msg);
+ }
+
nh.spinOnce();
}
View
2 ihe_hardware/turtlebot_button/manifest.xml
@@ -12,6 +12,8 @@
<depend package="rosserial_client"/>
<depend package="rosserial_msgs"/>
<depend package="rosserial_python"/>
+ <depend package="roscpp"/>
+ <depend package="geometry_msgs"/>
</package>
View
1 ihe_hardware/turtlebot_button/msg/Buttons.msg
@@ -1,3 +1,2 @@
uint8[] button_state
-uint8 estop_state
View
104 ihe_hardware/turtlebot_button/src/turtlebot_button.cpp
@@ -0,0 +1,104 @@
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+#include <turtlebot_button/Buttons.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+#include <vector>
+#include "boost/thread/mutex.hpp"
+#include "boost/thread/thread.hpp"
+#include "ros/console.h"
+
+const int NBUTTONS = 5;
+
+class TurtlebotButtonOp
+{
+public:
+ TurtlebotButtonOp();
+
+private:
+ void buttonCallback(const turtlebot_button::Buttons::ConstPtr& button);
+ void estopCallback(const std_msgs::Bool::ConstPtr& estop);
+ void publish();
+
+ ros::NodeHandle ph, nh;
+ ros::Publisher twist_pub;
+ ros::Subscriber button_sub;
+ ros::Subscriber estop_sub;
+
+ uint8_t last_button_state[NBUTTONS];
+ uint8_t estop_state;
+
+ geometry_msgs::Twist last_published;
+ boost::mutex publish_mutex;
+ ros::Timer timer;
+};
+
+TurtlebotButtonOp::TurtlebotButtonOp() {
+ twist_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+ button_sub = nh.subscribe<turtlebot_button::Buttons>("buttons", 100, &TurtlebotButtonOp::buttonCallback, this);
+ estop_sub = nh.subscribe<std_msgs::Bool>("estop", 1, &TurtlebotButtonOp::estopCallback, this);
+
+ timer = nh.createTimer(ros::Duration(0.5), boost::bind(&TurtlebotButtonOp::publish, this));
+}
+
+void TurtlebotButtonOp::buttonCallback(const turtlebot_button::Buttons::ConstPtr& button) {
+ geometry_msgs::Twist vel;
+ uint8_t i;
+
+ for (i = 0; i < button->button_state.size(); i++) {
+ if ((last_button_state[i] != button->button_state[i]) && button->button_state[i]) {
+ switch (i) {
+ case 0:
+ vel.angular.z = 1;
+
+ break;
+ case 1:
+ vel.linear.x = 1;
+
+ break;
+ case 2:
+ vel.linear.x = -1;
+
+ break;
+ case 3:
+ vel.angular.z = -1;
+
+ break;
+ case 4:
+
+
+ break;
+ default:
+
+ break;
+ }
+ }
+ }
+
+ last_published = vel;
+
+ for (uint8_t i = 0; i < button->button_state.size(); i++) {
+ last_button_state[i] = button->button_state[i];
+ }
+}
+
+void TurtlebotButtonOp::estopCallback(const std_msgs::Bool::ConstPtr& estop) {
+ estop_state = estop->data;
+}
+
+void TurtlebotButtonOp::publish() {
+ boost::mutex::scoped_lock lock(publish_mutex);
+
+ if (estop_state) {
+ twist_pub.publish(last_published);
+ } else {
+
+ }
+}
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "turtlebot_button_op");
+ TurtlebotButtonOp turtlebot_button_op;
+
+ ros::spin();
+}
View
6 ihe_hardware/turtlebot_button/turtlebot_button.launch
@@ -0,0 +1,6 @@
+<launch>
+
+ <node pkg="turtlebot_button" type="turtlebot_button" name="turtlebot_button_op">
+ </node>
+
+</launch>

0 comments on commit 718f0c3

Please sign in to comment.