THRUSTERS: raise alarm on fatal thruster configuration #257

LucasBA · 2017-05-15T20:50:40Z

#252 THIS IS NOT READY FOR MERGE. I added a thruster kill for when we lose two linearly dependent thrusters. I couldn't think of a way to test it without running it on the sub and unplugging thrusters so I figured I would just post this to show what I have so far and to see if anyone had any input.

kev-the-dev

Got some Q's to be A'd about these implementations, but good stuff

kev-the-dev · 2017-05-16T16:02:10Z

command/sub8_missions/sub8/sub_singleton.py

+        pose command.
+        '''
+        for i in xrange(len(positions)):
+            yield self.sub.move.look_at_without_pitching(np.array(positions[i][0:3])).go(speed)


Why do we look at the point first? I think the desired behavior is one move setting relative position and orientation from the current position. This lets the controller pick how to get to this pose.

pose = self.sub.pose yield self.sub.move.relative(np.array(positions[0:3])).set_orientation(transformations.quaternion_multiply( self.sub.pose.orientation, transformations.quaternion_from_euler(orientations[i][0], orientations[i][1], orientations[i][2]))).go(speed)

kev-the-dev · 2017-05-16T16:06:51Z

drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py

+                update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout)
+                rospy.wait_for_service('b_matrix')
+                try:
+                    B = rospy.ServiceProxy('b_matrix', BMatrix)


I don't think this does what you want. This doesn't call the service, it returns an object to call it later. See rospy.ServiceProxy. Try:

# in __init__ so you only have to do this once rospy.wait_for_service('b_matrix') self.bmatrix_proxy = rospy.ServiceProxy('b_matrix', BMatrix) ... #In alert_thruster_unloss B = self.bmatrix_proxy() #You might have to put an empty BMatirxRequest() as an argument B = numpy.asmatrix(B.B)

Yeah thanks

kev-the-dev · 2017-05-16T16:17:02Z

command/sub8_missions/sub8/sub_singleton.py

@@ -312,6 +312,38 @@ def _run_look(self, spotings_req):

            yield self.sub.nh.sleep(.5)

+class PoseSequenceCommander(object):


I would prefer if this was refactored to calculate all the poses in the constructor and give the user control over when to run them. This would make it easy to plug into the Searcher class we have or other custom uses.
So something like:

def __init__(self, sub, positions, orientations): # Calculate poses into self.poses def move(self, **kwargs): for p in pose: p.go(**kwargs)

kev-the-dev · 2017-05-16T16:17:53Z

drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py

+                            },
+                            severity=5
+                        )
+                except rospy.ServiceException, e:


Do you need two try/except blocks? Could this all be in one block?

kev-the-dev · 2017-05-16T16:18:15Z

drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py

+                            severity=5
+                        )
+                except rospy.ServiceException, e:
+                    print "Service call failed: %s"%e


This should be a rospy.logwarn/rospy.logerr so it shows up in logs and should describe what service call is failing

kev-the-dev · 2017-05-16T16:20:24Z

command/sub8_missions/missions/pose_sequence_test.py

+def run(sub_singleton):
+    '''Used for testing in sim. These commands may not be safe for the real sub.'''
+    fprint("Starting Test With Eulers", msg_color="blue")
+    positions = [np.array([-1, 0, 0]), np.array([0, 0, 3]),


It'd be nice if I could just put normal ole' python arrays as the input to PoseSequenceCommander and it would convert to numpy for me so i don't have to type np.array a million times

kev-the-dev · 2017-05-22T21:04:11Z

A lot of these diffs are copied in #249. If you have multiple PRs in at once, try to have them both based on master so we don't have these duplicate diffs. Please rebase this ontop of master to only contain the changes to the thruster driver.

git fetch upstream/master
git rebase -i upstream/master
# in file that opens uncomment all commits but the last one.

kev-the-dev · 2017-06-02T16:17:36Z

What's up Lucas? If you rebase it to fix the git shenanigans we can run it in lab and see if it has the intended behavior.

LucasBA · 2017-06-05T23:43:41Z

I thought I fixed it but I am getting this c3 error when I catkin_make, Is this a known thing? Do I have something improperly configured?

/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp: In member function ‘void Node::timer_callback(const ros::TimerEvent&)’:
/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp:207:88: error: ‘const struct mil_msgs::MoveToGoal_<std::allocator >’ has no member named ‘blind’
goal->speed, !goal->uncoordinated, !goal->blind);
^
/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp:215:21: error: ‘mil_msgs::MoveToResult {aka struct mil_msgs::MoveToResult_<std::allocator >}’ has no member named ‘error’
actionresult_.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
^
/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp:216:21: error: ‘mil_msgs::MoveToResult {aka struct mil_msgs::MoveToResult_<std::allocator >}’ has no member named ‘success’
actionresult_.success = checkWPResult.first;
^
/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp:285:21: error: ‘mil_msgs::MoveToResult {aka struct mil_msgs::MoveToResult_<std::allocator >}’ has no member named ‘success’
actionresult_.success = false;
^
/home/lucas/mil_ws/src/SubjuGator/legacy/c3_trajectory_generator/src/node.cpp:286:21: error: ‘mil_msgs::MoveToResult {aka struct mil_msgs::MoveToResult_<std::allocator >}’ has no member named ‘error’
actionresult_.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
^
SubjuGator/legacy/c3_trajectory_generator/CMakeFiles/c3_trajectory_generator.dir/build.make:86: recipe for target 'SubjuGator/legacy/c3_trajectory_generator/CMakeFiles/c3_trajectory_generator.dir/src/node.cpp.o' failed
make[2]: *** [SubjuGator/legacy/c3_trajectory_generator/CMakeFiles/c3_trajectory_generator.dir/src/node.cpp.o] Error 1
CMakeFiles/Makefile2:9243: recipe for target 'SubjuGator/legacy/c3_trajectory_generator/CMakeFiles/c3_trajectory_generator.dir/all' failed
make[1]: *** [SubjuGator/legacy/c3_trajectory_generator/CMakeFiles/c3_trajectory_generator.dir/all] Error 2
Makefile:138: recipe for target 'all' failed

RustyBamboo · 2017-06-06T00:58:08Z

@LucasBA pull the latest mil_common

LucasBA · 2017-06-06T02:47:03Z

Thanks that worked

sentree · 2017-06-15T18:15:41Z

Is this still waiting on changes, or does @LucasBA just need to fix the formatting?

LucasBA · 2017-06-15T19:43:50Z

David made a bunch of changes to this file and is still working on it so he wanted to stabilize that before merging this

sentree · 2017-07-18T04:46:55Z

The GNC stack has been merged back in and seems fairly stable. Is there any chance this could be quickly brought up to date before competition or is it now a more involved project? It would definitely be a nice feature to have seeing as our thrusters appear to be dying weekly now...

kev-the-dev · 2019-02-22T21:41:12Z

Archived at archive_pr_257

LucasBA force-pushed the degen branch from 4562c9b to a496de4 Compare May 16, 2017 14:03

kev-the-dev requested changes May 16, 2017

View reviewed changes

kev-the-dev mentioned this pull request May 22, 2017

MISSIONS: add class to execute consecutive moves #249

Merged

kev-the-dev changed the title ~~Degen~~ THRUSTERS: raise alarm on fatal thruster configuration May 22, 2017

DSsoto added the dont merge label May 23, 2017

LucasBA added 3 commits June 5, 2017 19:35

B_MATRIX KILL: Fix merge issue

5c26166

THRUSTER ALARM: Update B matrix in mapper when a thruster goes out

6c8a102

THRUSTER ALARM: Fix B matrix service call and rank check

d9268fb

LucasBA force-pushed the degen branch 2 times, most recently from 8e159b6 to 2154fe6 Compare June 6, 2017 15:58

THRUSTER ALARM: Clean up try logic

2876a0e

LucasBA force-pushed the degen branch from 2154fe6 to 2876a0e Compare June 6, 2017 16:13

kev-the-dev closed this Feb 22, 2019

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

THRUSTERS: raise alarm on fatal thruster configuration #257

THRUSTERS: raise alarm on fatal thruster configuration #257

LucasBA commented May 15, 2017 •

edited

Loading

kev-the-dev left a comment •

edited

Loading

kev-the-dev May 16, 2017

kev-the-dev May 16, 2017

LucasBA May 26, 2017

kev-the-dev May 16, 2017

kev-the-dev May 16, 2017

kev-the-dev May 16, 2017

kev-the-dev May 16, 2017

kev-the-dev commented May 22, 2017 •

edited

Loading

kev-the-dev commented Jun 2, 2017

LucasBA commented Jun 5, 2017

RustyBamboo commented Jun 6, 2017

LucasBA commented Jun 6, 2017

sentree commented Jun 15, 2017

LucasBA commented Jun 15, 2017

sentree commented Jul 18, 2017

kev-the-dev commented Feb 22, 2019

		@@ -312,6 +312,38 @@ def _run_look(self, spotings_req):

		yield self.sub.nh.sleep(.5)

		class PoseSequenceCommander(object):

THRUSTERS: raise alarm on fatal thruster configuration #257

THRUSTERS: raise alarm on fatal thruster configuration #257

Conversation

LucasBA commented May 15, 2017 • edited Loading

kev-the-dev left a comment • edited Loading

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

LucasBA May 26, 2017

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

kev-the-dev May 16, 2017

Choose a reason for hiding this comment

kev-the-dev commented May 22, 2017 • edited Loading

kev-the-dev commented Jun 2, 2017

LucasBA commented Jun 5, 2017

RustyBamboo commented Jun 6, 2017

LucasBA commented Jun 6, 2017

sentree commented Jun 15, 2017

LucasBA commented Jun 15, 2017

sentree commented Jul 18, 2017

kev-the-dev commented Feb 22, 2019

LucasBA commented May 15, 2017 •

edited

Loading

kev-the-dev left a comment •

edited

Loading

kev-the-dev commented May 22, 2017 •

edited

Loading