diff --git a/robot_high_level_control/scripts/state_machine.py b/robot_high_level_control/scripts/state_machine.py index c7b7d66..5a1081e 100755 --- a/robot_high_level_control/scripts/state_machine.py +++ b/robot_high_level_control/scripts/state_machine.py @@ -28,10 +28,13 @@ def __init__(self, starting_state="drive_to_p0"): # Variables to use for self.state_vars = { "outbound": True, - "P0": (2.0, 1.0), + "P0": (1.75, 1.0), "P1": (1.5, 2.0), "P2": (1.5, 4.0), "P3": (1.5, 5.0), + "PA1": (1.0, 1.0), + "PA2": (0.5, 1.0), + "PA3": (0.4, 1.0), "drivability_map": None, "max_x_offset": 1.5, "P1_y": 2.5, diff --git a/robot_misc_tools/scripts/tf_configs_static/full_system.json b/robot_misc_tools/scripts/tf_configs_static/full_system.json index bc9ce20..5e00542 100644 --- a/robot_misc_tools/scripts/tf_configs_static/full_system.json +++ b/robot_misc_tools/scripts/tf_configs_static/full_system.json @@ -4,7 +4,7 @@ "frame_id" : "camera_mount_base", "child_id" : "robot_frame_BL", "trans_x" : 0, - "trans_y" : 0, + "trans_y" : 0.235, "trans_z" : 0, "rot_x" : 0, "rot_y" : 0, @@ -26,6 +26,17 @@ "frame_id" : "robot_frame_BL", "child_id" : "robot_frame_BR", "trans_x" : 0, + "trans_y" : -0.45, + "trans_z" : 0, + "rot_x" : 0, + "rot_y" : 0, + "rot_z" : 0, + "rot_w" : 1 + }, + { + "frame_id" : "robot_frame_BL", + "child_id" : "robot_frame_FLJ", + "trans_x" : 0.845, "trans_y" : 0, "trans_z" : 0, "rot_x" : 0, @@ -36,7 +47,7 @@ { "frame_id" : "robot_frame_BL", "child_id" : "robot_frame_FL", - "trans_x" : 0, + "trans_x" : 1.07, "trans_y" : 0, "trans_z" : 0, "rot_x" : 0, @@ -47,7 +58,7 @@ { "frame_id" : "robot_frame_BR", "child_id" : "robot_frame_FR", - "trans_x" : 0, + "trans_x" : 0.845, "trans_y" : 0, "trans_z" : 0, "rot_x" : 0, diff --git a/robot_mission_control/launch/localization.launch b/robot_mission_control/launch/localization.launch index 064ad83..94d21db 100644 --- a/robot_mission_control/launch/localization.launch +++ b/robot_mission_control/launch/localization.launch @@ -1,10 +1,13 @@ + + + - - + + @@ -26,6 +29,13 @@ + + + + + + + diff --git a/robot_motor_control/cfg/Talon.cfg b/robot_motor_control/cfg/Talon.cfg index 3615898..fb3d8c8 100755 --- a/robot_motor_control/cfg/Talon.cfg +++ b/robot_motor_control/cfg/Talon.cfg @@ -8,6 +8,7 @@ gen = ParameterGenerator() gen.add("id", int_t, 0, "ID Number", 0, 0, 64) gen.add("inverted", bool_t, 0, "Inverted", False) gen.add("peak_voltage", double_t, 0, "Peak Voltage", 12.0, 0.0, 28) +gen.add("pot", bool_t, 0, "Potentiometer", False) gen.add("P", double_t, 0, "P", 0, 0, 100) gen.add("I", double_t, 0, "I", 0, 0, 100) diff --git a/robot_motor_control/include/robot_motor_control/TalonNode.h b/robot_motor_control/include/robot_motor_control/TalonNode.h index 4be5a5a..d815f17 100644 --- a/robot_motor_control/include/robot_motor_control/TalonNode.h +++ b/robot_motor_control/include/robot_motor_control/TalonNode.h @@ -24,6 +24,7 @@ namespace robot_motor_control{ ros::NodeHandle nh; std::string _name; dynamic_reconfigure::Server server; + TalonConfig _config; std::unique_ptr talon; @@ -42,9 +43,11 @@ namespace robot_motor_control{ ControlMode _controlMode; double _output; bool disabled; + bool configured; + bool not_configured_warned; public: - TalonNode(const ros::NodeHandle& parent, const std::string& name); + TalonNode(const ros::NodeHandle& parent, const std::string& name, const TalonConfig& config); TalonNode& operator=(const TalonNode&) = delete; @@ -52,7 +55,7 @@ namespace robot_motor_control{ void reconfigure(const TalonConfig &config, uint32_t level); - void updateConfig(const TalonConfig &config); + void configure(); void setPercentOutput(std_msgs::Float64 output); diff --git a/robot_motor_control/scripts/camera_motor_control.py b/robot_motor_control/scripts/camera_motor_control.py index 91180a8..c40f49b 100755 --- a/robot_motor_control/scripts/camera_motor_control.py +++ b/robot_motor_control/scripts/camera_motor_control.py @@ -6,23 +6,26 @@ class AngleListener: - def __init__(self, pinNumber): - - # Configure GPIO stuff - GPIO.setmode(GPIO.BOARD) - GPIO.setwarnings(False) + def __init__(self, pinNumber, max_angle): # Set the pin as an output self.p = GPIO.setup(pinNumber, GPIO.OUT) self.p = GPIO.PWM(pinNumber, 50) + # Store max angle to use for scaling + self.max_angle = max_angle + # Start self.p.start(2.5) def setCameraAngle(self, theta): - # Convert from range -90 to 90 to range 2.22 to 10.0 (this is the good range for the servo) - duty_cycle = ((theta.data + 65) / 130.0) * (7.88) + 2.22 + rospy.loginfo(theta.data) + + # Convert from range -90 to 90 to range 0 to 100 + duty_cycle = ((theta.data + self.max_angle) / (self.max_angle * 2)) * (7.5) + 2.5 + + rospy.loginfo(duty_cycle) # Send new duty cycle to servo self.p.ChangeDutyCycle(duty_cycle) @@ -30,11 +33,15 @@ def setCameraAngle(self, theta): if __name__ == '__main__': + # Initialize GPIO + GPIO.setmode(GPIO.BOARD) + GPIO.setwarnings(False) + # Initialize ROS node rospy.init_node('camera_control_node', anonymous=True) # Create listener object - listener = AngleListener(5) + listener = AngleListener(18, 135.0 / 2) # Attach subscriber to servo angle topic rospy.Subscriber("aruco/servo_theta", Int32, listener.setCameraAngle) diff --git a/robot_motor_control/scripts/digging_control.py b/robot_motor_control/scripts/digging_control.py index d525be8..5eeaf67 100755 --- a/robot_motor_control/scripts/digging_control.py +++ b/robot_motor_control/scripts/digging_control.py @@ -6,7 +6,7 @@ class DigManager: - def __init__(self): + def __init__(self, ): # Current dig mode - can be "teleop" or "auto" self.dig_mode = "teleop" diff --git a/robot_motor_control/src/MotorControl.cpp b/robot_motor_control/src/MotorControl.cpp index e8895d1..ede059c 100644 --- a/robot_motor_control/src/MotorControl.cpp +++ b/robot_motor_control/src/MotorControl.cpp @@ -25,6 +25,8 @@ int main(int argc, char **argv) { config.inverted = (bool)v["inverted"]; if(v.hasMember("peak_voltage")) config.peak_voltage = (double)v["peak_voltage"]; + if(v.hasMember("pot")) + config.pot = (bool)v["pot"]; if(v.hasMember("P")) config.P = (double)v["P"]; if(v.hasMember("I")) @@ -35,8 +37,7 @@ int main(int argc, char **argv) { config.F = (double)v["F"]; auto node = ros::NodeHandle(nh, name); - std::shared_ptr talon = std::make_shared(node, name); - talon->updateConfig(config); + std::shared_ptr talon = std::make_shared(node, name, config); talons.push_back(talon); ROS_INFO("Created Talon with name '%s' and id '%d'", name.c_str(), config.id); }else{ diff --git a/robot_motor_control/src/TalonNode.cpp b/robot_motor_control/src/TalonNode.cpp index 62371ab..a3597f2 100644 --- a/robot_motor_control/src/TalonNode.cpp +++ b/robot_motor_control/src/TalonNode.cpp @@ -10,8 +10,8 @@ using namespace ctre::phoenix::motorcontrol::can; namespace robot_motor_control { - TalonNode::TalonNode(const ros::NodeHandle& parent, const std::string& name) : - nh(parent), _name(name), server(nh), talon(new TalonSRX(0)), + TalonNode::TalonNode(const ros::NodeHandle& parent, const std::string& name, const TalonConfig& config): + nh(parent), _name(name), server(nh), _config(config), talon(new TalonSRX(_config.id)), tempPub(nh.advertise("temperature", 1)), busVoltagePub(nh.advertise("bus_voltage", 1)), outputPercentPub(nh.advertise("output_percent", 1)), @@ -21,10 +21,11 @@ namespace robot_motor_control { velPub(nh.advertise("velocity", 1)), setPercentSub(nh.subscribe("set_percent_output", 1, &TalonNode::setPercentOutput, this)), setVelSub(nh.subscribe("set_velocity", 1, &TalonNode::setVelocity, this)), - lastUpdate(ros::Time::now()), _controlMode(ControlMode::PercentOutput), _output(0.0), disabled(false){ + lastUpdate(ros::Time::now()), _controlMode(ControlMode::PercentOutput), _output(0.0), disabled(false), + configured(false), not_configured_warned(false){ server.setCallback(boost::bind(&TalonNode::reconfigure, this, _1, _2)); + server.updateConfig(config); talon->Set(ControlMode::PercentOutput, 0); - configureStatusPeriod(*talon); } void TalonNode::setPercentOutput(std_msgs::Float64 output) { @@ -40,37 +41,61 @@ namespace robot_motor_control { } void TalonNode::reconfigure(const TalonConfig &config, uint32_t level) { - if (config.id != 0 && talon->GetDeviceID() != config.id) { - ROS_INFO("Resetting TalonNode to new id: %d", config.id); - talon = std::make_unique(config.id); - configureStatusPeriod(*talon); + this->_config = config; + this->configured = false; + this->configure(); + } + + void TalonNode::configure(){ + if (talon->GetDeviceID() != _config.id) { + ROS_INFO("Resetting TalonNode to new id: %d", _config.id); + talon = std::make_unique(_config.id); } - ROS_INFO("Reconfiguring Talon: %s", _name.c_str()); + + configureStatusPeriod(*talon); TalonSRXConfiguration c; SlotConfiguration slot; - slot.kP = config.P; - slot.kI = config.I; - slot.kD = config.D; - slot.kF = config.F; + slot.kP = _config.P; + slot.kI = _config.I; + slot.kD = _config.D; + slot.kF = _config.F; c.slot0 = slot; - c.voltageCompSaturation = config.peak_voltage; + c.voltageCompSaturation = _config.peak_voltage; c.pulseWidthPeriod_EdgesPerRot = 4096; - talon->ConfigAllSettings(c); + ErrorCode error = talon->ConfigAllSettings(c, 50); + + if(_config.pot){ + talon->ConfigSelectedFeedbackSensor(FeedbackDevice::Analog); + }else{ + talon->ConfigSelectedFeedbackSensor(FeedbackDevice::CTRE_MagEncoder_Relative); + } talon->SelectProfileSlot(0,0); - talon->SetInverted(config.inverted); + talon->SetInverted(_config.inverted); talon->EnableVoltageCompensation(true); - } - void TalonNode::updateConfig(const TalonConfig &config){ - server.updateConfig(config); + if(error != ErrorCode::OK){ + if(!this->not_configured_warned){ + ROS_WARN("Reconfiguring Talon %s %d failed!", _name.c_str(), talon->GetDeviceID()); + this->not_configured_warned = true; + } + this->configured = false; + }else{ + ROS_INFO("Reconfigured Talon: %s with %d %f %f %f", _name.c_str(), talon->GetDeviceID(), + _config.P, _config.I, _config.D); + this->not_configured_warned = false; + this->configured = true; + } } void TalonNode::update(){ + if(!this->configured){ + this->configure(); + } if(ros::Time::now()-lastUpdate > ros::Duration(0.2)){ // Disable the Talon if we aren't getting commands - if(!this->disabled) ROS_INFO("Talon disabled for not receiving updates: %s", _name.c_str()); + if(!this->disabled) ROS_WARN("Talon disabled for not receiving updates: %s", _name.c_str()); this->disabled = true; this->_controlMode = ControlMode::PercentOutput; this->_output = 0.0; diff --git a/robot_simulation/rviz/competition_config.rviz b/robot_simulation/rviz/competition_config.rviz index d1e335e..cc45089 100644 --- a/robot_simulation/rviz/competition_config.rviz +++ b/robot_simulation/rviz/competition_config.rviz @@ -5,7 +5,6 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /TF1/Status1 Splitter Ratio: 0.5 Tree Height: 719 - Class: rviz/Selection @@ -99,13 +98,13 @@ Visualization Manager: Unreliable: false Value: true - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /vizualization/arena_markers Name: MarkerArray Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/TF Enabled: true Frame Timeout: 15 @@ -113,19 +112,27 @@ Visualization Manager: All Enabled: true aruco_board_origin: Value: true - aruco_camera: - Value: true camera_link: Value: true - robot_back_left: + camera_mount_base: Value: true - robot_back_right: + kinect_base: + Value: true + kinect_mount_base: + Value: true + localization_camera: Value: true robot_center: Value: true - robot_front_left: + robot_frame_BL: + Value: true + robot_frame_BR: + Value: true + robot_frame_FL: + Value: true + robot_frame_FLJ: Value: true - robot_front_right: + robot_frame_FR: Value: true world: Value: true @@ -137,18 +144,21 @@ Visualization Manager: Tree: world: aruco_board_origin: - aruco_camera: - robot_center: - camera_link: - {} - robot_back_left: - {} - robot_back_right: - {} - robot_front_left: - {} - robot_front_right: - {} + localization_camera: + camera_mount_base: + robot_frame_BL: + robot_center: + {} + robot_frame_BR: + robot_frame_FR: + {} + robot_frame_FL: + kinect_mount_base: + kinect_base: + camera_link: + {} + robot_frame_FLJ: + {} Update Interval: 0 Value: true - Alpha: 0.300000012 diff --git a/robot_slam/ekf/robot_pose_ekf.py b/robot_slam/ekf/robot_pose_ekf.py index 9c64648..41dfe41 100755 --- a/robot_slam/ekf/robot_pose_ekf.py +++ b/robot_slam/ekf/robot_pose_ekf.py @@ -39,7 +39,7 @@ def __init__(self): ''' # Define the measurement noise covariance matrix - self.arucoEKF.R = np.eye(7) * np.array([0.05, 0.05, 0.05, 0.01, 0.01, 0.01, 0.01]) + self.arucoEKF.R = np.eye(7) * np.array([0.05, 0.05, 0.05, 0.20, 0.20, 0.20, 0.20]) # Define the process noise covariance matrix #arucoEKF.Q = @@ -154,7 +154,7 @@ def hx(self, x): ) # Set loop rate - r = rospy.Rate(10) # 10hz + r = rospy.Rate(20) # 10hz while not rospy.is_shutdown(): posePublisher.publish(robot_pose_ekf.getPose()) diff --git a/robot_slam/localization/aruco_camera_centering.py b/robot_slam/localization/aruco_camera_centering.py index 00f8b35..74e275b 100755 --- a/robot_slam/localization/aruco_camera_centering.py +++ b/robot_slam/localization/aruco_camera_centering.py @@ -33,8 +33,8 @@ def __init__(self): # Servo state self.theta = Int32() # ROS object that stores current servo theta orientation self.error = 0 # Error between current theta and where the camera should be - self.minTheta = -65 # Minimum servo bound - self.maxTheta = 65 # Maximum servo bound + self.minTheta = -(135.0 / 2) # Minimum servo bound + self.maxTheta = (135.0 / 2) # Maximum servo bound # Marker paramters self.markerDetected = False # True if an AruCo marker is in view @@ -93,7 +93,11 @@ def updatePosition(self): camera_to_mount.position.x = 0 camera_to_mount.position.y = 0 camera_to_mount.position.z = 0 - camera_to_mount.orientation = tf.transformations.quaternion_from_euler(0, 0, radians(self.theta.data)) + quaternion = tf.transformations.quaternion_from_euler(0, 0, -radians(self.theta.data)) + camera_to_mount.orientation.x = quaternion[0] + camera_to_mount.orientation.y = quaternion[1] + camera_to_mount.orientation.z = quaternion[2] + camera_to_mount.orientation.w = quaternion[3] self.lensPosePub.publish(camera_to_mount) diff --git a/robot_slam/localization/aruco_localization.py b/robot_slam/localization/aruco_localization.py index 70acc18..4a2e598 100755 --- a/robot_slam/localization/aruco_localization.py +++ b/robot_slam/localization/aruco_localization.py @@ -36,19 +36,20 @@ # default locations of the aruco markers -- # aruco marker on the short wall of the arena (1) aruco_marker_south_pos_default = { - "trans_x": 1.5, # TODO: Change. - "trans_y": 0.0, + "trans_x": 0.0, # TODO: Change. + "trans_y": 0.5, "trans_z": 1.0, # TODO: Change. - "rot_w": SQRT_2 / 2.0, + "rot_w": 1.0, # SQRT_2 / 2.0, "rot_x": 0.0, "rot_y": 0.0, - "rot_z": SQRT_2 / 2.0, # 90-degree rotation in z + "rot_z": 0.0, #SQRT_2 / 2.0, # 90-degree rotation in z } south_pose = Pose() # gets initialized in __init__ + # aruco marker on the collection bin (2) aruco_marker_bin_pos_default = { "trans_x": 0.0, # TODO: Change. - "trans_y": 0.6, + "trans_y": 0.715, "trans_z": 1.0, # TODO: Change. "rot_w": 1.0, "rot_x": 0.0, @@ -56,11 +57,12 @@ "rot_z": 0.0, } bin_pose = Pose() + # small aruco marker on the collection bin (3) aruco_marker_smbin_pos_default = { "trans_x": 0.0, # TODO: Change. "trans_y": 0.5, - "trans_z": 1.1, # TODO: Change. + "trans_z": 0.915, # TODO: Change. "rot_w": 1.0, "rot_x": 0.0, "rot_y": 0.0, @@ -98,12 +100,14 @@ def __init__(self): south_marker_len = 0.168 south_marker_sep = 0.1 south_marker_idstart = 810 + # bin board bin_markers_x = 1 bin_markers_y = 1 bin_marker_len = 0.168 bin_marker_sep = 0.1 bin_marker_idstart = 800 + # small bin board smbin_markers_x = 5 smbin_markers_y = 1 @@ -203,6 +207,8 @@ def __init__(self): # Callback for received image frames def image_callback(self, data): + self.update_aruco_target() + # Define variables for messages to publish pose_msg = Pose() bool_msg = Bool() @@ -229,7 +235,7 @@ def image_callback(self, data): # If succesful, convert rvec from rpy to quaternion, fill pose message if retval: - quat = tf.transformations.quaternion_from_euler(rvec[1], rvec[0], rvec[2]) + quat = tf.transformations.quaternion_from_euler(rvec[1] + math.pi, rvec[0], rvec[2] + math.pi) # Store pose and quaternion information of Aruco Board pose_msg.position.x = tvec[2] @@ -309,7 +315,7 @@ def update_aruco_target(self): rospy.loginfo("changing to board 1") elif self.marker_number == SMALL_BIN_MARKER: - if x > y and x < 2.5: + if x > y and x > 2.5: self.marker_number = BIN_MARKER # big bin board takes priority self.active_board = self.bin_board rospy.loginfo("changing to board 2") @@ -335,9 +341,4 @@ def update_aruco_target(self): # Create subscriber for aruco image frames handler = ImageHandler() - # update the aruco target every once in a while - loop_rate = rospy.Rate(1) - # Run indefinitely - while not rospy.is_shutdown(): - handler.update_aruco_target() - loop_rate.sleep() + rospy.spin()