11#include < chrono>
2- #include < functional>
3- #include < memory>
4- #include < string>
5-
6- #include " rclcpp/rclcpp.hpp"
7- #include " std_msgs/msg/string.hpp"
8- #include " std_msgs/msg/float64_multi_array.hpp"
2+ #include < std_msgs/msg/float64_multi_array.hpp>
93#include < sensor_msgs/msg/joint_state.hpp>
104#include < std_msgs/msg/multi_array_dimension.hpp>
5+ #include < rclcpp/rclcpp.hpp>
6+ #include < std_msgs/msg/string.hpp>
117
128using namespace std ::chrono_literals;
139
1410#define INCREMENT 0.1 // rads
1511
16- std::vector<std::string> name_order = { " shoulder_pan_joint" , " shoulder_lift_joint" , " elbow_joint" ,
12+ std::vector<std::string> NAME_ORDER = { " shoulder_pan_joint" , " shoulder_lift_joint" , " elbow_joint" ,
1713 " wrist_1_joint" , " wrist_2_joint" , " wrist_3_joint" };
1814
1915class MinimalDriverTest : public rclcpp ::Node
@@ -23,14 +19,14 @@ class MinimalDriverTest : public rclcpp::Node
2319 {
2420 publisher_ =
2521 this ->create_publisher <std_msgs::msg::Float64MultiArray>(" forward_command_controller_position/commands" , 10 );
26- timer_ = this ->create_wall_timer (1500ms, std::bind (&MinimalDriverTest::timer_callback , this ));
22+ timer_ = this ->create_wall_timer (1500ms, std::bind (&MinimalDriverTest::timerCallback , this ));
2723
2824 subscription_ = this ->create_subscription <sensor_msgs::msg::JointState>(
29- " joint_states" , 10 , std::bind (&MinimalDriverTest::topic_callback , this , std::placeholders::_1));
25+ " joint_states" , 10 , std::bind (&MinimalDriverTest::topicCallback , this , std::placeholders::_1));
3026 }
3127
3228private:
33- void timer_callback ()
29+ void timerCallback ()
3430 {
3531 if (!js_received_)
3632 {
@@ -52,19 +48,19 @@ class MinimalDriverTest : public rclcpp::Node
5248 message.layout .dim [0 ].stride = 1 ;
5349 message.layout .dim [0 ].label = " cmd" ;
5450
55- for (size_t i = 0 ; i < name_order .size (); ++i)
51+ for (size_t i = 0 ; i < NAME_ORDER .size (); ++i)
5652 {
5753 for (size_t j = 0 ; j < msg_internal_.name .size (); ++j)
5854 {
59- if (msg_internal_.name [j] == name_order [i])
55+ if (msg_internal_.name [j] == NAME_ORDER [i])
6056 message.data [i] = msg_internal_.position [j] + INCREMENT;
6157 }
6258 }
6359
6460 publisher_->publish (message);
6561 }
6662
67- void topic_callback (const sensor_msgs::msg::JointState::SharedPtr msg)
63+ void topicCallback (const sensor_msgs::msg::JointState::SharedPtr msg)
6864 {
6965 if (js_received_)
7066 return ;
@@ -86,4 +82,4 @@ int main(int argc, char* argv[])
8682 rclcpp::spin (std::make_shared<MinimalDriverTest>());
8783 rclcpp::shutdown ();
8884 return 0 ;
89- }
85+ }
0 commit comments