Merge pull request ros-drivers#24 from msoe-vex/navx-over-ros

Fix ROS Connection for NavX
msoe-vex · Mar 2, 2021 · 98fe103 · 98fe103
2 parents d0db168 + 7fc4012
commit 98fe103
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 25 deletions.
diff --git a/src/change_up_driver_control/launch/15_inch_robot.launch b/src/change_up_driver_control/launch/15_inch_robot.launch
@@ -1,6 +1,6 @@
 <launch>
   <!-- the primary port that allows rosserial to function -->
-  <arg name="mainport" default="/dev/ttyACM0" />
+  <arg name="mainport" default="/dev/ttyACM1" />
   <arg name="mainbaud" default="57600" />
 
   <node name="connector" type="serial_node.py" pkg="rosserial_arduino" respawn="true" output="screen">
@@ -9,7 +9,7 @@
   </node>
 
   <!-- Node for NavX data -->
-  <node name="navx_publisher_node" type="navx_publisher_node" pkg="navx_publisher" output="screen">
+  <node name="navx_publisher_node" type="navx_publisher_node" pkg="navx_publisher" respawn="true" output="screen">
     <param name="device" value="/dev/ttyACM2"/>
   </node>
 </launch>
diff --git a/src/navx_publisher/src/navx_publisher.cpp b/src/navx_publisher/src/navx_publisher.cpp
@@ -239,11 +239,8 @@ int main(int argc, char **argv)
 			tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
 			const double dTime = odom.header.stamp.toSec() - last_time.toSec();
 			imu_msg.angular_velocity.x = roll / dTime;
-			imu_rpy_msg.roll = roll;
 			imu_msg.angular_velocity.y = pitch / dTime;
-			imu_rpy_msg.pitch = pitch;
 			imu_msg.angular_velocity.z = -yaw / dTime;
-			imu_rpy_msg.yaw = yaw;
 			imu_msg_raw.angular_velocity = imu_msg.angular_velocity;
 			last_rot = pose;
 			last_time = odom.header.stamp;
@@ -262,6 +259,11 @@ int main(int argc, char **argv)
 
 			odom.twist.twist.angular = imu_msg.angular_velocity;
 
+			// Gather RPY into custom message
+			imu_rpy_msg.roll = nx.GetRoll();
+			imu_rpy_msg.pitch = nx.GetPitch();
+			imu_rpy_msg.yaw = nx.GetYaw();
+
 			//publish to ROS topics
 			time_pub.publish(timestamp);
 			imu_pub.publish(imu_msg);

diff --git a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp
@@ -21,17 +21,18 @@ DriverControlNode::DriverControlNode(NodeManager* node_manager, MotorNode* left_
     rear_module_location(0) = rear_module_location_x;
     rear_module_location(1) = rear_module_location_y;
 
-    // m_navx_sub = new ros::Subscriber<v5_hal::RollPitchYaw, DriverControlNode>
-    //     ("/navx/rpy", &DriverControlNode::m_navxDataCallback, this);
+    robot_angle = Eigen::Rotation2Dd(0);
+
+    m_navx_sub = new ros::Subscriber<v5_hal::RollPitchYaw, DriverControlNode>
+        ("/navx/rpy", &DriverControlNode::m_navxDataCallback, this);
 }
 
 void DriverControlNode::initialize() {
-
+    Node::m_handle->subscribe(*m_navx_sub);
 }
 
 void DriverControlNode::m_navxDataCallback(const v5_hal::RollPitchYaw& msg) {
-	//robot_angle = Eigen::Rotation2Dd(msg.yaw);
-    Node::m_handle->loginfo("Message Recieved");
+	robot_angle = Eigen::Rotation2Dd((msg.yaw * (M_PI/180)));
 }
 
 /**
@@ -63,19 +64,13 @@ void DriverControlNode::m_spinEjectionRollerVoltage(int voltage) {
 	ejection_roller->moveVoltage(voltage);
 }
 
-void DriverControlNode::periodic() {
-    // printf("Inertial Sensor Yaw: %f\n", inertial_sensor->getYaw());
-    double sensor_yaw = inertial_sensor->getYaw();
-    robot_angle = Eigen::Rotation2Dd(-sensor_yaw);
-
+void DriverControlNode::periodic() {    
     controller_target_velocity(0) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X) / 127.0) * max_velocity;
     controller_target_velocity(1) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 127.0) * max_velocity;
     rotation_velocity = -(controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 127.0) * max_rotation_velocity;
 
     field_target_velocity = robot_angle.inverse() * controller_target_velocity;
 
-    // printf("Field Target Velocity: %f\n", field_target_velocity);
-
     swerveController.assignActualAngle(left_swerve_pot->getValue(), right_swerve_pot->getValue(), rear_swerve_pot->getValue());
 
     MotorPowers left_motor_powers = swerveController.calculateLeftModule(field_target_velocity, rotation_velocity);
@@ -89,13 +84,6 @@ void DriverControlNode::periodic() {
     rear_swerve_1->move(rear_motor_powers.left_motor_power);
     rear_swerve_2->move(rear_motor_powers.right_motor_power);
 
-    // printf("Left Motor 1 Power: %d\n", left_motor_powers.left_motor_power);
-    // printf("Left Motor 2 Power: %d\n", left_motor_powers.right_motor_power);
-    // printf("Right Motor 1 Power: %d\n", right_motor_powers.left_motor_power);
-    // printf("Right Motor 2 Power: %d\n", right_motor_powers.right_motor_power);
-    // printf("Rear Motor 1 Power: %d\n", rear_motor_powers.left_motor_power);
-    // printf("Rear Motor 2 Power: %d\n", rear_motor_powers.right_motor_power);
-
     int intake_voltage = 0;
 	int main_rollers_voltage = 0;
 	int ejection_roller_voltage = 0;

diff --git a/src/v5_hal/firmware/src/main.cpp b/src/v5_hal/firmware/src/main.cpp
@@ -59,7 +59,7 @@ void initialize() {
 	ejection_roller = new MotorNode(node_manager, 15, "ejectionRoller", false);
 	top_rollers = new MotorNode(node_manager, 7, "topRollers", true);
 
-	inertial_sensor = new InertialSensorNode(node_manager, 9, "inertialSensor");
+	//inertial_sensor = new InertialSensorNode(node_manager, 9, "inertialSensor");
 
 	x_odometry_encoder = new ADIEncoderNode(node_manager, 1, 2, "xOdometryEncoder", false);
 	y_odometry_encoder = new ADIEncoderNode(node_manager, 3, 4, "yOdometryEncoder", false);

diff --git a/startup/start-ros.sh b/startup/start-ros.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+echo "Starting ROS..."
+cd /home/ubuntu/ChangeUp
+source ./devel/setup.bash
+roslaunch change_up_driver_control 15_inch_robot.launch