Skip to content

Commit ea7dcb9

Browse files
gbr1gbr1
committed
gpio digitaloutput added
1 parent 8bea899 commit ea7dcb9

File tree

6 files changed

+215
-2
lines changed

6 files changed

+215
-2
lines changed

‎CMakeLists.txt

+16-2
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,8 @@ find_package(catkin REQUIRED COMPONENTS
3838

3939
## Generate messages in the 'msg' folder
4040
add_message_files(FILES
41+
ListGpio.msg
42+
Gpio.msg
4143
Leds.msg
4244
Led.msg
4345
)
@@ -89,11 +91,20 @@ target_link_libraries(led_test
8991
${catkin_LIBRARIES}
9092
)
9193

94+
add_executable(gpio_node
95+
src/gpio_node.cpp
96+
)
97+
add_dependencies(gpio_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
98+
target_link_libraries(gpio_node
99+
mraa
100+
${catkin_LIBRARIES}
101+
)
102+
92103
#############
93104
## Install ##
94105
#############
95106

96-
install(TARGETS led_node led_test
107+
install(TARGETS led_node led_test gpio_node
97108
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
98109
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
99110
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -106,4 +117,7 @@ install(DIRECTORY src/
106117
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src )
107118

108119
install(DIRECTORY msg/
109-
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/msg )
120+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/msg )
121+
122+
install(DIRECTORY config/
123+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config )

‎config/example_gpio.yaml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# The MIT License
2+
#
3+
# Copyright (c) 2019 Giovanni di Dio Bruno https://gbr1.github.io
4+
#
5+
# Permission is hereby granted, free of charge, to any person obtaining a copy
6+
# of this software and associated documentation files (the "Software"), to deal
7+
# in the Software without restriction, including without limitation the rights
8+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9+
# copies of the Software, and to permit persons to whom the Software is
10+
# furnished to do so, subject to the following conditions:
11+
#
12+
# The above copyright notice and this permission notice shall be included in
13+
# all copies or substantial portions of the Software.
14+
#
15+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18+
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21+
# THE SOFTWARE.
22+
#
23+
#
24+
upboard_ros:
25+
DigitalOut: [22,32]

‎launch/gpio.launch

+30
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0"?>
2+
<!--
3+
The MIT License
4+
5+
Copyright (c) 2019 Giovanni di Dio Bruno https://gbr1.github.io
6+
7+
Permission is hereby granted, free of charge, to any person obtaining a copy
8+
of this software and associated documentation files (the "Software"), to deal
9+
in the Software without restriction, including without limitation the rights
10+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11+
copies of the Software, and to permit persons to whom the Software is
12+
furnished to do so, subject to the following conditions:
13+
14+
The above copyright notice and this permission notice shall be included in
15+
all copies or substantial portions of the Software.
16+
17+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
23+
THE SOFTWARE.
24+
-->
25+
<launch>
26+
27+
<rosparam command="load" file="$(find upboard_ros)/config/example_gpio.yaml" />
28+
<node pkg="upboard_ros" type="gpio_node" name="gpio" output="screen">
29+
</node>
30+
</launch>

‎msg/Gpio.msg

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# The MIT License
2+
#
3+
# Copyright (c) 2019 Giovanni di Dio Bruno https://gbr1.github.io
4+
#
5+
# Permission is hereby granted, free of charge, to any person obtaining a copy
6+
# of this software and associated documentation files (the "Software"), to deal
7+
# in the Software without restriction, including without limitation the rights
8+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9+
# copies of the Software, and to permit persons to whom the Software is
10+
# furnished to do so, subject to the following conditions:
11+
#
12+
# The above copyright notice and this permission notice shall be included in
13+
# all copies or substantial portions of the Software.
14+
#
15+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18+
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21+
# THE SOFTWARE.
22+
#
23+
#
24+
uint8 pin
25+
uint8 value

‎msg/ListGpio.msg

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# The MIT License
2+
#
3+
# Copyright (c) 2019 Giovanni di Dio Bruno https://gbr1.github.io
4+
#
5+
# Permission is hereby granted, free of charge, to any person obtaining a copy
6+
# of this software and associated documentation files (the "Software"), to deal
7+
# in the Software without restriction, including without limitation the rights
8+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9+
# copies of the Software, and to permit persons to whom the Software is
10+
# furnished to do so, subject to the following conditions:
11+
#
12+
# The above copyright notice and this permission notice shall be included in
13+
# all copies or substantial portions of the Software.
14+
#
15+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18+
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21+
# THE SOFTWARE.
22+
#
23+
#
24+
Header header
25+
Gpio[] gpio

‎src/gpio_node.cpp

+94
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
#include <ros/ros.h>
2+
3+
#include "mraa/common.hpp"
4+
#include "mraa/gpio.hpp"
5+
6+
#include <upboard_ros/Gpio.h>
7+
#include <upboard_ros/ListGpio.h>
8+
9+
#include <vector>
10+
11+
class pGpio : public mraa::Gpio{
12+
private:
13+
int _pin;
14+
15+
public:
16+
pGpio(int pin, bool owner=true, bool raw=false):mraa::Gpio(pin,owner,raw){
17+
_pin=pin;
18+
}
19+
int getPin(){return _pin;}
20+
~pGpio() {};
21+
};
22+
23+
std::vector<pGpio*> listgpio_output;
24+
25+
26+
void setGpio(uint8_t pin, uint8_t value){
27+
int k=0;
28+
bool found=false;
29+
while((!found) && (k<listgpio_output.size())){
30+
if (listgpio_output[k]->getPin()==pin){
31+
found=true;
32+
try{
33+
listgpio_output[k]->write(value);
34+
}
35+
catch(std::exception& e){
36+
ROS_ERROR("%s",e.what());
37+
}
38+
}
39+
k++;
40+
}
41+
if (!found){
42+
ROS_WARN("GPIO %d is not defined. Check your configs",pin);
43+
}
44+
}
45+
46+
47+
void gpioCallback(const upboard_ros::ListGpio & msg){
48+
for (int i=0; i<msg.gpio.size(); i++){
49+
setGpio(msg.gpio[i].pin,msg.gpio[i].value);
50+
}
51+
}
52+
53+
int main(int argc, char **argv){
54+
ros::init(argc, argv, "upboard_ros/gpio_node");
55+
ros::NodeHandle nh;
56+
57+
/* rostopic pub /upboard/gpio/write upboard_ros/ListGpio "{header: auto, gpio:[{pin: 22, value: 0}]}" --once */
58+
59+
std::vector<int> paramlist;
60+
61+
//load digital output pins
62+
if (nh.getParam("upboard_ros/DigitalOut",paramlist)){
63+
for (int i=0; i<paramlist.size(); i++){
64+
try{
65+
pGpio* tmp = new pGpio(paramlist[i]);
66+
tmp->dir(mraa::DIR_OUT);
67+
listgpio_output.push_back(tmp);
68+
69+
ROS_INFO("Pin %d is configured as Digital Output",paramlist[i]);
70+
}
71+
catch(std::exception& e){
72+
ROS_ERROR("%s",e.what());
73+
}
74+
}
75+
}
76+
77+
78+
ros::Subscriber gpiosub = nh.subscribe("/upboard/gpio/write", 10, gpioCallback);
79+
80+
ros::spin();
81+
82+
83+
//put all outputs to 0
84+
for (int i=0; i<listgpio_output.size(); i++){
85+
try{
86+
listgpio_output[i]->write(0);
87+
}
88+
catch(std::exception& e){
89+
ROS_ERROR("%s",e.what());
90+
}
91+
}
92+
93+
return 0;
94+
}

0 commit comments

Comments
 (0)