Fixed the gpiochip numbering change in going from Jetpack 4.x to Jetpack 5.x

catkin_ws/src/asclinic_pkg/CMakeLists.txt

@@ -172,6 +172,9 @@ add_executable(i2c_for_motors_and_servos             src/nodes/i2c_for_motors_an
                                                      src/drivers/src/i2c_driver/i2c_driver.cpp
                                                      src/drivers/src/pololu_smc_g2/pololu_smc_g2.cpp
                                                      src/drivers/src/pca9685/pca9685.cpp)
+add_executable(i2c_for_motors                        src/nodes/i2c_for_motors.cpp
+                                                     src/drivers/src/i2c_driver/i2c_driver.cpp
+                                                     src/drivers/src/pololu_smc_g2/pololu_smc_g2.cpp)
 add_executable(i2c_for_servos                        src/nodes/i2c_for_servos.cpp
                                                      src/drivers/src/i2c_driver/i2c_driver.cpp
                                                      src/drivers/src/pca9685/pca9685.cpp)
@@ -201,6 +204,7 @@ add_dependencies(template_gpio_event_triggered         asclinic_pkg_generate_mes
 add_dependencies(template_gpio_polling                 asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(encoder_read_multi_threaded           asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(i2c_for_motors_and_servos             asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(i2c_for_motors                        asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(i2c_for_servos                        asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(i2c_for_sensors                       asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(emulate_i2c_for_motors                asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -219,6 +223,7 @@ target_link_libraries(template_gpio_event_triggered         ${catkin_LIBRARIES}
 target_link_libraries(template_gpio_polling                 ${catkin_LIBRARIES} -lgpiod)
 target_link_libraries(encoder_read_multi_threaded           ${catkin_LIBRARIES} -lgpiod)
 target_link_libraries(i2c_for_motors_and_servos             ${catkin_LIBRARIES})
+target_link_libraries(i2c_for_motors                        ${catkin_LIBRARIES})
 target_link_libraries(i2c_for_servos                        ${catkin_LIBRARIES})
 target_link_libraries(i2c_for_sensors                       ${catkin_LIBRARIES})
 target_link_libraries(emulate_i2c_for_motors                ${catkin_LIBRARIES})

catkin_ws/src/asclinic_pkg/launch/encoder.launch

@@ -14,6 +14,10 @@
 			output = "screen"
 			type   = "encoder_read_multi_threaded"
 			>
+			<param
+				name   = "gpiochip_number"
+				value  = "1"
+			/>
 			<param
 				name   = "line_number_for_motor_left_channel_a"
 				value  = "133"
@@ -48,9 +52,9 @@
 		<!-- LAUNCH A "I2C for Motors and Servos" NODE -->
 		<node
 			pkg    = "asclinic_pkg"
-			name   = "i2c_for_motors_and_servos"
+			name   = "i2c_for_motors"
 			output = "screen"
-			type   = "i2c_for_motors_and_servos"
+			type   = "i2c_for_motors"
 		/>
 
 	</group>

catkin_ws/src/asclinic_pkg/src/nodes/encoder_read_multi_threaded.cpp

@@ -61,6 +61,9 @@ int m_encoder_counts_for_motor_right_b = 0;
 // > Mutex for preventing multiple-access of shared variables
 std::mutex m_counting_mutex;
 
+// > The "gpiochip" number
+int m_gpiochip_number = 1;
+
 // > The line numbers to read
 int m_line_number_for_motor_left_channel_a = 133;
 int m_line_number_for_motor_left_channel_b = 134;
@@ -167,7 +170,9 @@ void encoderCountingThreadMain()
 	// Specify the chip name of the GPIO interface
 	// > Note: for the 40-pin header of the Jetson SBCs, this
 	//   is "/dev/gpiochip0"
-	const char * gpio_chip_name = "/dev/gpiochip0";
+	std::stringstream temp_string_stream;
+	temp_string_stream << "/dev/gpiochip" << m_gpiochip_number;
+	const char * gpio_chip_name = temp_string_stream.str().c_str();
 
 	// Make a local copy of the line number member variables
 	int line_number_left_a  = m_line_number_for_motor_left_channel_a;
@@ -327,6 +332,14 @@ int main(int argc, char* argv[])
 	// > Thus, to access this parameter, we first get a handle to
 	//   this node within the namespace that it was launched.
 	//
+
+	// Get the "gpiochip" number parameter:
+	if ( !nodeHandle.getParam("gpiochip_number", m_gpiochip_number) )
+	{
+		// Display an error message
+		ROS_INFO("[ENCODER READ MULTI THREADED] FAILED to get \"gpiochip_number\" parameter. Using default value instead.");
+	}
+
 	// Get the line number parameters:
 	// > For channel A of the left side motor
 	if ( !nodeHandle.getParam("line_number_for_motor_left_channel_a", m_line_number_for_motor_left_channel_a) )
@@ -396,6 +409,9 @@ int main(int argc, char* argv[])
 	// Spin the node
 	ros::spin();
 
+	// Set the flag to stop counting
+	encoder_thread_should_count = false;
+
 	// Join back the encoder counting thread
 	encoder_counting_thread.join();
 

catkin_ws/src/asclinic_pkg/src/nodes/i2c_for_motors.cpp

+// Copyright (C) 2021, The University of Melbourne, Department of Electrical and Electronic Engineering (EEE)
+//
+// This file is part of ASClinic-System.
+//    
+// See the root of the repository for license details.
+//
+// ----------------------------------------------------------------------------
+//     _    ____   ____ _ _       _          ____            _                 
+//    / \  / ___| / ___| (_)____ (_) ___    / ___| _   _ ___| |_ ___ ________  
+//   / _ \ \___ \| |   | | |  _ \| |/ __|___\___ \| | | / __| __/ _ \  _   _ \ 
+//  / ___ \ ___) | |___| | | | | | | (_|_____|__) | |_| \__ \ ||  __/ | | | | |
+// /_/   \_\____/ \____|_|_|_| |_|_|\___|   |____/ \__, |___/\__\___|_| |_| |_|
+//                                                 |___/                       
+//
+// DESCRIPTION:
+// Node for I2C bus with Pololu SMC devices connected
+//
+// ----------------------------------------------------------------------------
+
+
+
+
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include "std_msgs/UInt16.h"
+#include "std_msgs/Float32.h"
+
+#include "i2c_driver/i2c_driver.h"
+
+#include "pololu_smc_g2/pololu_smc_g2.h"
+
+#include <bitset>
+
+// Include the asclinic message types
+#include "asclinic_pkg/LeftRightFloat32.h"
+
+// Namespacing the package
+//using namespace asclinic_pkg;
+
+
+
+
+
+// MEMBER VARIABLES FOR THIS NODE:
+// > For the I2C driver
+const char * m_i2c_device_name = "/dev/i2c-1";
+I2C_Driver m_i2c_driver (m_i2c_device_name);
+
+// > For the Pololu Simple Motor Controller (SMC) driver
+const uint8_t m_pololu_smc_address_left = 13;
+Pololu_SMC_G2 m_pololu_smc_left (&m_i2c_driver, m_pololu_smc_address_left);
+
+const uint8_t m_pololu_smc_address_right = 14;
+Pololu_SMC_G2 m_pololu_smc_right (&m_i2c_driver, m_pololu_smc_address_right);
+
+// > Publisher for the current duty cycle
+//   setting of the main drive motors
+ros::Publisher m_current_motor_duty_cycle_publisher;
+
+
+
+// Respond to subscriber receiving a message
+// > To test this out without creating an additional
+//   ROS node
+//   1) First use the command:
+//        rostopic list
+//      To identify the full name of this subscription topic.
+//   2) Then use the following command to send  message on
+//      that topic
+//        rostopic pub --once <namespace>/set_motor_duty_cycle asclinic_pkg/LeftRightFloat32 "{left: 10.1, right: 10.1}"
+//      where "<namespace>/set_motor_duty_cycle" is the full
+//      name identified in step 1.
+//
+void driveMotorsSubscriberCallback(const asclinic_pkg::LeftRightFloat32& msg)
+{
+	ROS_INFO_STREAM("[I2C FOR MOTORS] Message received with left = " << msg.left << ", right = " << msg.right);
+
+	// Clip the data to be in the range [-100.0,100.0]
+	// > For the left value
+	float pwm_duty_cycle_left = msg.left;
+	if (pwm_duty_cycle_left < -100.0f)
+		pwm_duty_cycle_left = -100.0f;
+	if (pwm_duty_cycle_left > 100.0f)
+		pwm_duty_cycle_left = 100.0f;
+	// > For the right value
+	float pwm_duty_cycle_right = msg.right;
+	if (pwm_duty_cycle_right < -100.0f)
+		pwm_duty_cycle_right = -100.0f;
+	if (pwm_duty_cycle_right > 100.0f)
+		pwm_duty_cycle_right = 100.0f;
+
+	// Initialise one boolean variable for the result
+	// of all calls to Pololu_SMC_G2 functions
+	bool result;
+
+	// SET THE TARGET DUTY CYCLE FOR EACH MOTOR CONTROLLER
+	// > NOTE: it may be necessary to negate one or both
+	//   of the duty cycles depending on the polarity
+	//   with which each motor is plugged in
+
+	// > Set the LEFT motor controller
+	result = m_pololu_smc_left.set_motor_target_duty_cycle_percent(pwm_duty_cycle_left);
+	if (!result)
+		ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED - Pololu SMC - set motor percent NOT successful for I2C address " << static_cast<int>(m_pololu_smc_left.get_i2c_address()) );
+
+	// > Set the RIGHT motor controller
+	result = m_pololu_smc_right.set_motor_target_duty_cycle_percent(-pwm_duty_cycle_right);
+	if (!result)
+		ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED - Pololu SMC - set motor percent NOT successful for I2C address " << static_cast<int>(m_pololu_smc_right.get_i2c_address()) );
+
+	// Publish the motor duty cycles
+	asclinic_pkg::LeftRightFloat32 msg_current_duty_cycle;
+	msg_current_duty_cycle.left  = pwm_duty_cycle_left;
+	msg_current_duty_cycle.right = pwm_duty_cycle_right;
+	m_current_motor_duty_cycle_publisher.publish(msg_current_duty_cycle);
+
+}
+
+
+
+int main(int argc, char* argv[])
+{
+	// Initialise the node
+	ros::init(argc, argv, "i2c_for_motors");
+	ros::NodeHandle nodeHandle("~");
+
+	// Initialise a node handle to the group namespace
+	std::string ns_for_group = ros::this_node::getNamespace();
+	ros::NodeHandle nh_for_group(ns_for_group);
+
+	// Initialise a subscriber for the duty cycle of the main drive motors
+	ros::Subscriber set_motor_duty_cycle_subscriber = nh_for_group.subscribe("set_motor_duty_cycle", 1, driveMotorsSubscriberCallback);
+
+	// Initialise a publisher for the current duty cycle setting of the main drive motors
+	m_current_motor_duty_cycle_publisher = nh_for_group.advertise<asclinic_pkg::LeftRightFloat32>("current_motor_duty_cycle", 10, true);
+
+	// Display command line command for publishing a
+	// motor duty cycle request
+	ROS_INFO_STREAM("[I2C FOR MOTORS] publish motor duty cycle requests from command line with: rostopic pub --once " << ros::this_node::getNamespace() << "/set_motor_duty_cycle asclinic_pkg/LeftRightFloat32 \"{left: 10.1, right: 10.1}\"");
+
+	// Open the I2C device
+	// > Note that the I2C driver is already instantiated
+	//   as a member variable of this node
+	bool open_success = m_i2c_driver.open_i2c_device();
+
+	// Display the status
+	if (!open_success)
+	{
+		ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED to open I2C device named " << m_i2c_driver.get_device_name());
+	}
+	else
+	{
+		ROS_INFO_STREAM("[I2C FOR MOTORS] Successfully opened named " << m_i2c_driver.get_device_name() << ", with file descriptor = " << m_i2c_driver.get_file_descriptor());
+	}
+
+
+
+	// NOTE:
+	// > The drivers are already instantiated as
+	//   member variables of this node for:
+	//   > Each of the Pololu simple motor controllers (SMC)
+
+
+
+	// SET THE CONFIGURATION OF EACH MOTOR CONTROLLER
+
+	// Specify the various limits
+	int new_current_limit_in_milliamps = 5000;
+	int new_max_duty_cycle_limit = 2560;
+	int new_max_accel_limit = 1;
+	int new_max_decel_limit = 5;
+
+	// Initialise a pointer for iterating through
+	// the Pololu SMC objects
+	Pololu_SMC_G2 * pololu_smc_pointer;
+
+	// Initialise each of the Pololu SMC objects
+	// with the limits specified above.
+
+	// Iterate over the pololu objects
+	for (int i_smc=0;i_smc<2;i_smc++)
+	{
+		// Point to the appropriate motor controller
+		if (i_smc==0)
+			pololu_smc_pointer = &m_pololu_smc_left;
+		else
+			pololu_smc_pointer = &m_pololu_smc_right;
+
+		// Display the object about to be initialised
+		ROS_INFO_STREAM("[I2C FOR MOTORS] Now initialising SMC with I2C address " << static_cast<int>(pololu_smc_pointer->get_i2c_address()) );
+
+		// Check if a device exists at the address
+		bool this_pololu_smc_is_connected = m_i2c_driver.check_for_device_at_address(pololu_smc_pointer->get_i2c_address());
+
+		if (this_pololu_smc_is_connected)
+		{
+			// Call the Pololu SMC initialisation function
+			bool verbose_display_for_SMC_init = false;
+			bool result_smc_init = pololu_smc_pointer->initialise_with_limits(new_current_limit_in_milliamps,new_max_duty_cycle_limit,new_max_accel_limit,new_max_decel_limit,verbose_display_for_SMC_init);
+
+			// Display if an error occurred
+			if (!result_smc_init)
+			{
+				ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED - while initialising SMC with I2C address " << static_cast<int>(pololu_smc_pointer->get_i2c_address()) );
+			}
+		}
+		else
+		{
+			// Display that the device is not connected
+			ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED - SMC device NOT detected at I2C address " << static_cast<int>(pololu_smc_pointer->get_i2c_address()) );
+		}
+	}
+
+	// Spin as a single-threaded node
+	ros::spin();
+
+	// Close the I2C device
+	bool close_success = m_i2c_driver.close_i2c_device();
+	
+	// Display the status
+	if (!close_success)
+	{
+		ROS_INFO_STREAM("[I2C FOR MOTORS] FAILED to close I2C device named " << m_i2c_driver.get_device_name());
+	}
+	else
+	{
+		ROS_INFO_STREAM("[I2C FOR MOTORS] Successfully closed device named " << m_i2c_driver.get_device_name());
+	}
+
+	return 0;
+}

install/install.sh

@@ -645,7 +645,7 @@ then
 	# > Add the "udev" rule
 	echo "# udev rules for giving gpio port access to the gpiod group" | sudo tee -a /etc/udev/rules.d/60-gpiod.rules
 	echo "# This allows use of certain libgpiod functions without sudo" | sudo tee -a /etc/udev/rules.d/60-gpiod.rules
-	echo "SUBSYSTEM==\"gpio\", KERNEL==\"gpiochip0\", GROUP=\"gpiod\", MODE=\"0660\"" | sudo tee -a /etc/udev/rules.d/60-gpiod.rules
+	echo "SUBSYSTEM==\"gpio\", KERNEL==\"gpiochip1\", GROUP=\"gpiod\", MODE=\"0660\"" | sudo tee -a /etc/udev/rules.d/60-gpiod.rules
 	# > Display the "udev" rule that was added
 	echo ""
 	echo ">> As a double check, the full contents of \"60-gpiod.rules\" is now:"
@@ -663,12 +663,22 @@ then
 
 	# Inform the user, and check that the installation worked
 	echo ""
-	echo ">> The GPIO congiruation is now complete."
+	echo ">> The GPIO configuration is now complete."
 	echo ">> Now checking the installation was successful by running the command:"
 	echo ">> sudo gpiodetect"
 	echo ""
 	sudo gpiodetect
 
+	echo ""
+	echo ">> You should check that the gpiochip added to the udev rules"
+	echo "   is the gpiochip that you actually need to access for your"
+	echo "   robot. The gpiod command line tools can assist you in"
+	echo "   figuring this out, i.e:"
+	echo "      \"gpiodetect\", \"gpioinfo\", \"gpioget\", \"gpioset\", \"gpiomon\""
+	echo "   For example: the upgrade from the Nvidia Jetpack 4.x to 5.x"
+	echo "   rearranges the gpiochip numbering."
+
+
 	echo ""
 	echo ">> The \"udev\" rules comes into effect after a restart,"
 	echo "   you can then check that the rule is working correctly"