diff --git a/src/CMakeLists.txt.jinja b/src/CMakeLists.txt.jinja index 6e60be7..4538b00 100644 --- a/src/CMakeLists.txt.jinja +++ b/src/CMakeLists.txt.jinja @@ -192,6 +192,13 @@ include_directories( # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) +{% if "CPP" in LANGUAGES %} +## Declare a C++ executable +add_executable(talker src/main.cpp) +target_link_libraries(talker ${catkin_LIBRARIES}) +add_dependencies(talker beginner_tutorials_generate_messages_cpp) +{% endif %} + {%if "Python" in LANGUAGES%} catkin_install_python(PROGRAMS scripts/main.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/src/Dockerfile.jinja b/src/Dockerfile.jinja index d8047b9..d3235e5 100644 --- a/src/Dockerfile.jinja +++ b/src/Dockerfile.jinja @@ -1,6 +1,7 @@ # Stage 1: Build Stage FROM git.spirirobotics.com/spiri/services-ros1-core:main AS builder + #We need to make sure to source the ROS environment in our dockerfile SHELL ["/bin/bash", "-c"] @@ -19,6 +20,9 @@ RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && rosdep install --from-pat RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make" RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make install" +RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc +RUN echo "source /root/catkin_ws/devel/setup.bash" >> ~/.bashrc + WORKDIR /root/catkin_ws RUN apt-get clean diff --git a/src/src/.gitkeep b/src/{%if 'CPP' in LANGUAGES%}src{%endif%}/.gitkeep similarity index 100% rename from src/src/.gitkeep rename to src/{%if 'CPP' in LANGUAGES%}src{%endif%}/.gitkeep diff --git a/src/{%if 'CPP' in LANGUAGES%}src{%endif%}/main.cpp b/src/{%if 'CPP' in LANGUAGES%}src{%endif%}/main.cpp new file mode 100644 index 0000000..9f54ca4 --- /dev/null +++ b/src/{%if 'CPP' in LANGUAGES%}src{%endif%}/main.cpp @@ -0,0 +1,85 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +#include + +/** + * This tutorial demonstrates simple sending of messages over the ROS system. + */ +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "talker"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + ros::Publisher chatter_pub = n.advertise("chatter", 1000); + + ros::Rate loop_rate(10); + + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ + int count = 0; + while (ros::ok()) + { + /** + * This is a message object. You stuff it with data, and then publish it. + */ + std_msgs::String msg; + + std::stringstream ss; + ss << "hello world " << count; + msg.data = ss.str(); + + ROS_INFO("%s", msg.data.c_str()); + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + chatter_pub.publish(msg); + + ros::spinOnce(); + + loop_rate.sleep(); + ++count; + } + + + return 0; +} diff --git a/src/src/{%if 'Python' in LANGUAGES%}scripts{%endif%}/main.py b/src/{%if 'Python' in LANGUAGES%}scripts{%endif%}/main.py similarity index 100% rename from src/src/{%if 'Python' in LANGUAGES%}scripts{%endif%}/main.py rename to src/{%if 'Python' in LANGUAGES%}scripts{%endif%}/main.py