def start(self):
self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_trafo)
array_data = [now.secs,now.nsecs,\
x,y,psi,x_vel,y_vel,psi_vel,\
x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]
data_to_send = Float64MultiArray(data = array_data)
pub_data.publish(data_to_send)
elif(joy_button_L1 == 1 or joy_button_R1 == 1):
twist.linear.x = joy_x
twist.linear.y = joy_y
twist.angular.z = joy_z
pub_velocity.publish(twist)
else:
# ----- idle if no goals -----
pub_velocity.publish(Twist())
if __name__ == '__main__':
rospy.init_node('controller', anonymous=True)
# PUBLISHER
pub_velocity = rospy.Publisher('/mallard/thruster_command',Twist,queue_size=10)
pub_data = rospy.Publisher('/mallard/controller_data',Float64MultiArray,queue_size=10)
# SUBSCRIBER
rospy.Subscriber("/joy",Joy,joy_callback)
# rospy.Subscriber("/slam_out_pose",PoseStamped,slam_callback)
rospy.Subscriber("/vicon_pose",PoseStamped,slam_callback)
rospy.Subscriber("/mallard/goals",Float64MultiArray,goal_callback)
rospy.Timer(rospy.Duration(0.1), control_callback,oneshot=False)
rospy.spin()
def __init__(self):
#init stuff
#get stuff
self.num_landmarks = 12
# Estimator stuff
# x = pn, pe, pd, phi, theta, psi
self.xhat = np.zeros((9 + 3 * self.num_landmarks, 1))
self.xhat_odom = Odometry()
# Covariance matrix
self.P = np.zeros(
(9 + 3 * self.num_landmarks, 9 + 3 * self.num_landmarks))
self.P[9:, 9:] = np.eye(3 * self.num_landmarks) * 9999999.9 # Inf
self.Q = np.diag([10.0, 5.0, 5.0]) # meas noise
# Measurements stuff
# Truth
self.truth_pn = 0.0
self.truth_pe = 0.0
self.truth_pd = 0.0
self.truth_phi = 0.0
self.truth_theta = 0.0
self.truth_psi = 0.0
self.truth_p = 0.0
self.truth_q = 0.0
self.truth_r = 0.0
self.truth_pndot = 0.0
self.truth_pedot = 0.0
self.truth_pddot = 0.0
self.truth_u = 0.0
self.truth_v = 0.0
self.truth_w = 0.0
self.prev_time = 0.0
self.imu_az = 0.0
#aruco Stuff
self.aruco_location = {
100: [0.0, 0.0, 0.0],
101: [0.0, -14.5, 5.0],
102: [5.0, -14.5, 5.0],
103: [-5.0, -14.5, 5.0],
104: [0.0, 14.5, 5.0],
105: [5.0, 14.5, 5.0],
106: [-5.0, 14.5, 5.0],
107: [7.0, 0.0, 5.0],
108: [7.0, -7.5, 5.0],
109: [7.0, 7.5, 5.0],
110: [-7.0, 0.0, 5.0],
111: [-7.0, -7.5, 5.0],
112: [-7.0, 7.5, 5.0],
}
# Number of propagate steps
self.N = 5
#Constants
self.g = 9.8
# ROS Stuff
# Init subscribers
self.truth_sub_ = rospy.Subscriber(
'/slammer/ground_truth/odometry/NED', Odometry,
self.truth_callback)
self.imu_sub_ = rospy.Subscriber('/slammer/imu/data', Imu,
self.imu_callback)
self.aruco_sub = rospy.Subscriber('/aruco/measurements',
MarkerMeasurementArray,
self.aruco_meas_callback)
# Init publishers
self.estimate_pub_ = rospy.Publisher('/ekf_estimate',
Odometry,
queue_size=10)
# # Init Timer
self.pub_rate_ = 100. #
self.update_timer_ = rospy.Timer(rospy.Duration(1.0 / self.pub_rate_),
self.pub_est)
def _on_fire_alarm_recv(self, msg, who: str):
if self.supervision_state == SupervisorNodeState.Alarm_recv:
self.timer = rospy.Timer(rospy.Duration(secs=0, nsecs=500000000),
self.timer_callback,
oneshot=True)
def on_saop_plan(self, msg: Plan):
if self.supervision_state == SupervisorNodeState.Planning:
self.timer = rospy.Timer(rospy.Duration(secs=0, nsecs=500000000),
self.timer_callback,
oneshot=True)
def __init__(self):
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
False)
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.max_speed = rospy.get_param('~max_speed', 0.5)
self.max_angular = rospy.get_param('~max_angular', 1.0)
self.publish_current = rospy.get_param('~publish_current', True)
self.has_preroll = rospy.get_param('~use_preroll', True)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', False)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.timer = rospy.Timer(
rospy.Duration(0.1),
self.timer_check) # stop motors if no cmd_vel received > 1second
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_timer = rospy.Timer(
rospy.Duration(0.05), self.timer_current
) # publish motor currents at 10Hz, read at 50Hz
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.odom_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)),
self.timer_odometry)
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
rotate_control.name = "move_y"
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# add the control to the interactive marker
int_marker.controls.append(rotate_control);
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
if __name__=="__main__":
rospy.init_node("basic_controls")
br = TransformBroadcaster()
# create a timer to update the published transforms
rospy.Timer(rospy.Duration(0.01), frameCallback)
server = InteractiveMarkerServer("basic_controls")
menu_handler.insert( "First Entry", callback=processFeedback )
menu_handler.insert( "Second Entry", callback=processFeedback )
sub_menu_handle = menu_handler.insert( "Submenu" )
menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback )
menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback )
# Adjust yaw
'''
position = Point(-3, 3, 0)
make6DofMarker( False, InteractiveMarkerControl.NONE, position, True)
position = Point( 0, 3, 0)
def main():
update_timer = rospy.Timer(rospy.Duration(10), update_vehicle_list)
def __init__(self):
self.rate = rospy.Rate(2)
self.task_number = 0
self.error_step = 0
# Tablet data
self.object_name = ""
self.video_step_url = ""
self.video_full_url = ""
self.watch_video_url = ""
self.mac_address = get_mac()
self.is_goto_active = False
self.goto_success = False
self.goto_type = None
self.is_error_correction_done = False
self.is_error_corrected = False
self.use_robot = True
self.use_tablet = True
self.teleop_only = False
self.is_robot_moving = False
self.test = True
if rospy.has_param("ras"):
ras = rospy.get_param("ras")
self.use_robot = ras['use_robot']
self.use_tablet = ras['use_tablet']
self.teleop_only = ras['teleop_only']
self.test = ras['is_test']
self.test_error = False
self.use_location = False
if rospy.has_param("adl"):
adl = rospy.get_param("adl")
self.test_error = adl['test_error']
self.use_location = adl['use_location']
self.do_error = SimpleActionServer(
'do_error', DoErrorAction,
execute_cb=self.do_error_execute,
auto_start=False)
# Called from the tablet when we want to go to a particular object
# This then forwards it to our Go To node via the self.goto_client
if self.use_tablet:
self.tablet = SimpleActionServer(
'tablet_response', TabletAction,
execute_cb=self.tablet_execute,
auto_start=False)
self.tablet.start()
if self.use_robot:
# Forward commands through our Go To node
self.goto_client = SimpleActionClient(
'goto', GotoAction)
self.goto_client.wait_for_server()
# Allow returning to base from the experimenter interface
self.goto_base = SimpleActionServer(
'goto_base', TabletAction, # Same as action, but ignore data in message
execute_cb=self.goto_base_execute,
auto_start=False)
self.goto_base.start()
self.is_robot_moving = False
self.do_error.start()
rospy.on_shutdown(self.shutdown)
# Initiates CASAS logger
rospy.Timer(rospy.Duration(0.01), self.casas_logging, oneshot=True)
# Log system information to CASAS
rospy.Timer(rospy.Duration(1), self.system_log, oneshot=True)
if self.use_robot or self.teleop_only:
rospy.Subscriber('/cmd_vel', Twist, self.robot_cmd_vel_cb)
self.battery_voltage = None
rospy.Subscriber('/sensor_state', SensorState, self.robot_sensor_state_cb)
rospy.Timer(rospy.Duration(2), self.robot_battery_cb, oneshot=True)
self.tf = TransformListener()
rospy.Timer(rospy.Duration(2), self.robot_location_cb, oneshot=True)
rospy.logwarn("No odom data received!Exit!")
return True
else:
return False
if __name__ == "__main__":
rospy.init_node("DrawingTraj")
de = DrawTraj()
# gazebo's odom, without nosie
rospy.Subscriber("odom", Odometry, de.odomCallBack)
# rf2o_odometry's odom
rospy.Subscriber("odom_rf2o", Odometry, de.rf2oCallBack)
rospy.sleep(2)
rospy.Timer(rospy.Duration(3), de.cbMonitor)
while not rospy.is_shutdown():
if de.cbMonitor(plt) == True:
break
ax1 = plt.subplot(1, 2, 1)
plt.title("odom display")
plt.plot(de.x_odom, de.y_odom, color="b", label="/imu_traj")
plt.plot(de.x_rf2o, de.y_rf2o, color="r", label="/rf2o_traj")
plt.legend()
# down-sampling for gazebo's /odom
rate = len(de.y_odom) / len(de.y_rf2o)
print rate, len(de.x_rf2o)
ax2 = plt.subplot(1, 2, 2)
errs = []
host.ip = socket.gethostbyname(host.hostname)
# If the host is pingable, mark it as online
try:
subprocess.check_output(["ping", "-c1", host.ip])
host.status = "Online"
# If pinging the host is unsuccessful, mark it as offline
except:
host.status = "Offline"
# If hostname resolution fails, the IP address is set the unknown
except:
host.ip = "Unknown"
host.status = "Unknown"
self.hosts.hosts.append(host)
def publish(self, event):
'''
Publishes the list of hosts and the information gathered about them.
'''
self.check_hosts()
self.pub_hosts.publish(self.hosts)
if __name__ == "__main__":
monitor = HostMonitor()
rospy.Timer(rospy.Duration(10), monitor.publish, oneshot=False)
rospy.spin()
def object_detector_cb(self, msg):
'''
Callback object_detector_cb (rostopic pub /object_detector_mqtt_msg decenter_msgs/ObjectDetector )
'''
# rospy.logdebug(
rospy.loginfo('object_detector_cb received:: %s' % msg)
#if there are not objects return
if len(msg.objects) <= 0:
rospy.logwarn('object_detector_cb::objects is empty')
return
robot_data = filter(
lambda obj: obj.get('id') == int(msg.metadata.robot_id),
self.robots)
if not self.send_pictures_enabled:
rospy.logwarn('Sending picture is not enabled, doing nothing')
return
robot_enable_service = robot_data[0]['enable_service']
self.robot_enable_service = robot_enable_service
self.enable_send_pictures(enable=False, service=robot_enable_service)
#so far we just take the first object detected
object_type = msg.objects[0].warehouse_obj_class
rospy.logdebug('Identified the object: %s' % object_type)
#switch between 3 cases: 'other', 'robot', 'person'
if object_type == 'person':
rospy.loginfo('Person Detected !')
self.send_person_alert(int(msg.metadata.robot_id))
elif object_type == 'robot':
rospy.loginfo('Robot Detected !')
if not self.send_robot_indicator(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
#so far the node to disable is hardcoded but parametrized
if not self.get_current_mission(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
if not self.cancel_mission(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
rospy.sleep(5)
if not self.enable_node(self.node_selected):
self.object_detector_fail(robot_enable_service)
return
if not self.unblock_node(node=self.node_selected,
robot_id=int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
if not self.disable_node(self.node_selected):
self.object_detector_fail(robot_enable_service)
return
if not self.insert_last_mission():
self.object_detector_fail(robot_enable_service)
return
if not self.wait_until_robot_takes_new_mission(
int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
rospy.Timer(period=rospy.Duration(self.node_enable_wait_time),
callback=self.node_enable_timer_callback,
oneshot=True)
elif object_type == 'others':
rospy.loginfo('Other thing Detected')
if not self.send_others_indicator(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
#so far the node to disable is hardcoded but parametrized
if not self.get_current_mission(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
if not self.cancel_mission(int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
rospy.sleep(5)
if not self.enable_node(self.node_selected):
self.object_detector_fail(robot_enable_service)
return
if not self.unblock_node(node=self.node_selected,
robot_id=int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
if not self.disable_node(self.node_selected):
self.object_detector_fail(robot_enable_service)
return
if not self.insert_last_mission():
self.object_detector_fail(robot_enable_service)
return
if not self.wait_until_robot_takes_new_mission(
int(msg.metadata.robot_id)):
self.object_detector_fail(robot_enable_service)
return
else:
rospy.logwarn(
'Indeterminate case. Not person, not robot, not others')
self.enable_send_pictures(enable=True,
service=robot_enable_service)
return
# Success
rospy.loginfo('Succeed')
rospy.Timer(period=rospy.Duration(self.img_enable_wait_time),
callback=self.enable_image_timer_callback,
oneshot=True)
# rospy.sleep(30)
# self.enable_send_pictures(
# enable=True,
# service=robot_enable_service
# )
return
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
## parameters
self.map_frame = rospy.get_param("~map_frame", 'odom')
self.bot_frame = 'base_link'
self.switch_thred = 1.5 # change to next lane if reach next
self.pursuit = PurePursuit()
self.lka = rospy.get_param("~lookahead", 0.5)
self.pursuit.set_look_ahead_distance(self.lka)
## node path
while not rospy.has_param("/guid_path") and not rospy.is_shutdown():
rospy.loginfo("Wait for /guid_path")
rospy.sleep(0.5)
self.guid_path = rospy.get_param("/guid_path")
self.tag_ang_init = rospy.get_param("/guid_path_ang_init")
self.last_node = -1
self.next_node_id = None
self.all_anchor_ids = rospy.get_param("/all_anchor_ids")
self.joy_lock = False
self.get_goal = True
self.joint_ang = None
## set first tracking lane
self.pub_robot_speech = rospy.Publisher("speech_case",
String,
queue_size=1)
self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1)
rospy.sleep(1) # wait for the publisher to be set well
self.set_lane(True)
# variable
self.target_global = None
self.listener = tf.TransformListener()
# markers
self.pub_goal_marker = rospy.Publisher("goal_marker",
Marker,
queue_size=1)
self.pub_target_marker = rospy.Publisher("target_marker",
Marker,
queue_size=1)
# publisher, subscriber
self.pub_pid_goal = rospy.Publisher("pid_control/goal",
PoseStamped,
queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber("anchor_position",
PoseDirectional,
self.cb_goal,
queue_size=1)
sub_joy = rospy.Subscriber('joy_teleop/joy',
Joy,
self.cb_joy,
queue_size=1)
sub_fr = rospy.Subscriber('front_right/ranges',
DeviceRangeArray,
self.cb_range,
queue_size=1)
sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states',
JointState,
self.cb_joint,
queue_size=1)
#Don't update goal too often
self.last_update_goal = None
self.goal_update_thred = 0.001
self.hist_goal = np.array([])
self.normal = True
self.get_goal = True
self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking)
# print("init done")
# prevent sudden stop
self.last_plan = None
# keep searching until find path or reach search end
self.search_angle = 10 * math.pi / 180
self.search_max = 5
### stupid range drawing
# for using tf to draw range
br1 = tf2_ros.StaticTransformBroadcaster()
br2 = tf2_ros.StaticTransformBroadcaster()
# stf.header.frame_id = "uwb_back_left"
# stf.child_frame_id = "base_link"
# stf.transform.translation.x = -1*r_tag_points[0, 0]
# stf.transform.translation.y = -1*r_tag_points[0, 1]
# br1.sendTransform(stf)
# stf2 = stf
# stf2.header.stamp = rospy.Time.now()
# stf2.header.frame_id = "uwb_front_right"
# stf2.transform.translation.x = -1*r_tag_points[1, 0]
# stf2.transform.translation.y = -1*r_tag_points[1, 1]
# br2.sendTransform(stf2)
stf = TransformStamped()
stf.header.stamp = rospy.Time.now()
stf.transform.rotation.w = 1
stf.header.frame_id = "base_link"
stf.child_frame_id = "uwb_back_left"
stf.transform.translation.x = r_tag_points[0, 0]
stf.transform.translation.y = r_tag_points[0, 1]
stf2 = TransformStamped()
stf2.header.stamp = rospy.Time.now()
stf2.transform.rotation.w = 1
stf2.header.frame_id = "base_link"
stf2.child_frame_id = "uwb_front_right"
stf2.transform.translation.x = r_tag_points[1, 0]
stf2.transform.translation.y = r_tag_points[1, 1]
def launch_simulation(self):
rospy.loginfo(
"Experiment setup: waiting for unreal MAV simulation to setup...")
# Wait for unreal simulation to setup
if self.startup_timeout > 0.0:
try:
rospy.wait_for_message("unreal_simulation_ready", String,
self.startup_timeout)
except rospy.ROSException:
self.stop_experiment(
"Simulation startup failed (timeout after " +
str(self.startup_timeout) + "s).")
return
else:
rospy.wait_for_message("unreal_simulation_ready", String)
rospy.loginfo("Waiting for unreal MAV simulation to setup... done.")
# Launch planner (by service, every planner needs to advertise this service when ready)
rospy.loginfo("Waiting for planner to be ready...")
if self.startup_timeout > 0.0:
try:
rospy.wait_for_service(self.ns_planner + "/toggle_running",
self.startup_timeout)
except rospy.ROSException:
self.stop_experiment("Planner startup failed (timeout after " +
str(self.startup_timeout) + "s).")
return
else:
rospy.wait_for_service(self.ns_planner + "/toggle_running")
if self.planner_delay > 0:
rospy.loginfo(
"Waiting for planner to be ready... done. Launch in %d seconds.",
self.planner_delay)
rospy.sleep(self.planner_delay)
else:
rospy.loginfo("Waiting for planner to be ready... done.")
run_planner_srv = rospy.ServiceProxy(
self.ns_planner + "/toggle_running", SetBool)
run_planner_srv(True)
# Setup first measurements
self.eval_walltime_0 = time.time()
self.eval_rostime_0 = rospy.get_time()
# Evaluation init
if self.evaluate:
self.writelog("Succesfully started the simulation.")
# Dump complete rosparams for reference
subprocess.check_call([
"rosparam", "dump",
os.path.join(self.eval_directory, "rosparams.yaml"), "/"
])
self.writelog("Dumped the parameter server into 'rosparams.yaml'.")
self.eval_n_maps = 0
self.eval_n_pointclouds = 1
# Keep track of the (most recent) rosbag
bag_expr = re.compile(
'tmp_bag_\d{4}-\d{2}-\d{2}-\d{2}-\d{2}-\d{2}\.bag.'
) # Default names
bags = [
b for b in os.listdir(
os.path.join(os.path.dirname(self.eval_directory),
"tmp_bags")) if bag_expr.match(b)
]
bags.sort(reverse=True)
if len(bags) > 0:
self.writelog("Registered '%s' as bag for this simulation." %
bags[0])
self.eval_log_file.write("[FLAG] Rosbag: %s\n" %
bags[0].split('.')[0])
else:
rospy.logwarn("No tmpbag found. Is rosbag recording?")
# Periodic evaluation (call once for initial measurement)
self.eval_callback(None)
rospy.Timer(rospy.Duration(self.eval_frequency), self.eval_callback)
# Finish
rospy.loginfo("\n" + "*" * 39 +
"\n* Succesfully started the simulation! *\n" + "*" * 39)
def left_cb(self, msg):
self.mission_timer = rospy.Timer(rospy.Duration(2.5), self.run_left,
True)
return {"success": True, "message": ""}
def __init__(self):
self.dT = 0.005;
self.timenow = time.time()
self.oldtime = self.timenow
self.timenow = rospy.Time.now()
# publish the action and direction
self.FSM_action = rospy.Publisher('/action', String, self.actioncallback)
self.FSM_direction = rospy.Publisher('/direction', String, self.directioncallback)
# create loop
rospy.Timer(rospy.Duration(self.dT), self.loop, oneshot=False)
self.action = 'stand'
self.direction = 'left'
self.Wait = 1
self.Back = 0
self.TRight = 0
self.SLeft = 0
self.Forward = 0
self.TLeft = 0
self.SRight = 0
self.T0_EN = 0
self.T0 = 0
self.T1_EN = 0
self.T1 = 0
self.wait_time0 = 1
self.timing_time0 = 0
self.A_time0 = 0
self.B_time0 = 0
self.C_time0 = 0
self.D_time0 = 0
self.delta_t0 = 0
self.Start_time0 = 0
self.wait_time1 = 1
self.timing_time1 = 0
self.A_time1 = 0
self.B_time1 = 0
self.C_time1 = 0
self.D_time1 = 0
self.delta_t1 = 0
self.Start_time1 = 0
self.A = 0
self.B = 0
self.C = 0
self.D = 0
self.E = 0
self.F = 0
self.G = 0
self.H = 0
self.I = 0
self.J = 0
self.K = 0
self.L = 0
self.M = 0
self.N = 0
imu_msg.angular_velocity.x = gyro_x * 0.0174
imu_msg.angular_velocity.y = gyro_y * 0.0174
imu_msg.angular_velocity.z = gyro_z * 0.0174
imu_msg.header.stamp = rospy.Time.now()
imu_pub.publish(imu_msg)
temp_pub = None
imu_pub = None
if __name__ == '__main__':
rospy.init_node('imu_node')
bus = smbus.SMBus(rospy.get_param('~bus', 1))
ADDR = rospy.get_param('~device_address', 0x68)
if type(ADDR) == str:
ADDR = int(ADDR, 16)
IMU_FRAME = rospy.get_param('~imu_frame', 'imu_link')
bus.write_byte_data(ADDR, PWR_MGMT_1, 0)
temp_pub = rospy.Publisher('temperature', Temperature, queue_size=10)
imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=10)
imu_timer = rospy.Timer(rospy.Duration(0.02), publish_imu)
temp_timer = rospy.Timer(rospy.Duration(10), publish_temp)
rospy.spin()
#!/usr/bin/env python
import rospy
import tf
rospy.init_node('FBM2_mock')
rospy.loginfo('\nFBM2 helper: emitting tf testbed_origin->robot_markerset @120Hz where the robot is at (0,0) aligned with the origin.')
# Emit transform continuously: robot_markerset to testbed_origin
tf_broadcaster = tf.TransformBroadcaster()
def tf_callback(event):
tf_broadcaster.sendTransform(
(0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"robot_markerset",
"testbed_origin"
)
rospy.Timer(rospy.Duration(1.0/120.0), tf_callback) # 120Hz, like MoCap
rospy.spin()
def __init__(self):
self.sub = rospy.Subscriber("joy", Joy, self.callback)
self.pub = rospy.Publisher('fusion', Control, queue_size=1)
self.last_published_time = rospy.get_rostime()
self.last_published = None
self.timer = rospy.Timer(rospy.Duration(1. / 20.), self.timer_callback)
#!/usr/bin/env python
import rospy
from M02_RCDP_sangukbo import util
from std_msgs.msg import String
# Initialize the node with rospy
rospy.init_node('publisher_node')
# Create publisher
publisher = rospy.Publisher("~topic", String, queue_size=1)
# Define Timer callback
def callback(event):
msg = String()
msg.data = "%s is %s!" % (util.getName(), util.getStatus())
publisher.publish(msg)
# Read parameter
pub_period = rospy.get_param("~pub_period", 1.0)
# Create timer
rospy.Timer(rospy.Duration.from_sec(pub_period), callback)
# spin to keep the script for exiting
rospy.spin()
def __init__(self, args):
if len(args) == 0:
self.motors_lin_vel_scale = 0.5
self.motors_ang_vel_scale = 0.5
self.motors_cmd_topic = "joyop/cmd_vel"
elif len(args) == 1:
self.motors_lin_vel_scale = [-float(args[0]), float(args[0])]
self.motors_ang_vel_scale = [-float(args[0]), float(args[0])]
self.cmd_topic = "joyop/cmd_vel"
elif len(args) == 2:
self.motors_lin_vel_scale = [-float(args[0]), float(args[0])]
self.motors_ang_vel_scale = [-float(args[1]), float(args[1])]
self.cmd_topic = "joyop/cmd_vel"
elif len(args) == 3:
self.motors_lin_vel_scale = [-float(args[0]), float(args[0])]
self.motors_ang_vel_scale = [-float(args[1]), float(args[1])]
self.cmd_topic = args[2]
else:
rospy.logerr("Too many arguments! Expected 3 arguments at most ("
"linear_velocity_scale, angular_velocity_scale and "
"cmd_topic)")
exit(-1)
self.lac_scale = 0.14
self.xtion_yaw_range = [-0.7, 0.7]
self.xtion_pitch_range = [-0.45, 0.75]
self.picam_yaw_range = [-0.7, 0.7]
self.picam_pitch_range = [-0.6, 0.75]
self.motors_lin_vel = 0
self.motors_ang_vel = 0
self.angular_ang_vel = 0
self.lac_position = 0
self.xtion_yaw = 0
self.xtion_pitch = 0
self.picam_yaw = 0
self.picam_pitch = 0
self.motors_vel_pub = rospy.Publisher(self.motors_cmd_topic,
Twist,
queue_size=1)
self.lac_position_pub = rospy.Publisher('/linear_actuator/command',
Float64,
queue_size=1)
self.xtion_yaw_pub = rospy.Publisher('/kinect_yaw_controller/command',
Float64,
queue_size=1)
self.xtion_pitch_pub = rospy.Publisher(
'/kinect_pitch_controller/command', Float64, queue_size=1)
self.picam_yaw_pub = rospy.Publisher('/camera_effector/pan_command',
Float64,
queue_size=1)
self.picam_pitch_pub = rospy.Publisher('/camera_effector/tilt_command',
Float64,
queue_size=1)
joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
rospy.Timer(rospy.Duration(0.1), self.launch_joy_node, oneshot=True)
rospy.sleep(rospy.Duration(3))
rospy.Timer(rospy.Duration(1.0 / 5.0),
self.pub_callback,
oneshot=False)
rospy.spin()
def init(self):
rospy.init_node('hydrus_tracking_controller', anonymous=True)
self.__hydrus_controller_freq = rospy.get_param('~hydrus_controller_freq', 100.0)
self.__control_mode = rospy.get_param('~control_model', POS_VEL)
self.__mpc_flag = rospy.get_param('~mpc', False)
self.__mpc_horizon = rospy.get_param('~mpc_horizon', 1.0)
if self.__mpc_flag:
rospy.loginfo("[hydrus_tracking_controller] MPC mode")
self.__control_mode = MPC
self.__hydrus_odom = Odometry()
self.__object_odom = Odometry()
self.__primitive_params = PrimitiveParams()
self.__primitive_recv_flag = False
self.__hydrus_init_process = True
self.__object_odom_sub = rospy.Subscriber("/vision/object_odom", Odometry, self.__objectOdomCallback)
self.__hydrus_odom_sub = rospy.Subscriber("/uav/cog/odom", Odometry, self.__hydrusOdomCallback)
self.__primitive_params_sub = rospy.Subscriber("/track/primitive_params", PrimitiveParams, self.__primitiveParamsCallback)
self.__hydrus_motion_init_flag_pub = rospy.Publisher('/track/hydrus_motion_init_flag', Empty, queue_size=1)
self.__hydrus_start_pub = rospy.Publisher('/teleop_command/start', Empty, queue_size=1)
self.__hydrus_takeoff_pub = rospy.Publisher('/teleop_command/takeoff', Empty, queue_size=1)
self.__hydrus_nav_cmd_pub = rospy.Publisher("/uav/nav", FlightNav, queue_size=1)
self.__hydrus_joints_ctrl_pub = rospy.Publisher("/hydrus/joints_ctrl", JointState, queue_size=1)
self.__mpc_stop_flag_pub = rospy.Publisher('/mpc/stop_cmd', Bool, queue_size=1)
self.__mpc_target_waypoints_pub = rospy.Publisher('/mpc/target_waypoints', MpcWaypointList, queue_size=1)
rospy.sleep(1.0)
self.__hydrus_nav_cmd = FlightNav()
self.__hydrus_nav_cmd.header.stamp = rospy.Time.now()
self.__hydrus_nav_cmd.header.frame_id = "world"
self.__hydrus_nav_cmd.control_frame = self.__hydrus_nav_cmd.WORLD_FRAME
self.__hydrus_nav_cmd.target = self.__hydrus_nav_cmd.COG
self.__hydrus_nav_cmd.pos_xy_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_pos_x = 0.0
self.__hydrus_nav_cmd.target_pos_y = 0.0
self.__hydrus_nav_cmd.pos_z_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_pos_z = 2.0
self.__hydrus_nav_cmd.psi_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_psi = -math.pi / 2.0 # 0.0
self.__hydrus_joints_ctrl_msg = JointState()
self.__hydrus_joints_ctrl_msg.name.append("joint1")
self.__hydrus_joints_ctrl_msg.name.append("joint2")
self.__hydrus_joints_ctrl_msg.name.append("joint3")
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
## hydrus start and takeoff
if self.__hydrus_init_process:
rospy.sleep(1.0)
self.__hydrus_start_pub.publish(Empty())
rospy.sleep(1.0)
self.__hydrus_takeoff_pub.publish(Empty())
rospy.loginfo("Hydrus arming and takeoff command is sent.")
rospy.sleep(21.0)
rospy.loginfo("Hydrus takeoff finsihed.")
self.__hydrus_nav_cmd_pub.publish(self.__hydrus_nav_cmd)
rospy.sleep(9.0)
rospy.loginfo("Hydrus moved to initial position.")
if self.__control_mode == MPC:
mpc_stop_msg = Bool()
mpc_stop_msg.data = True
self.__mpc_stop_flag_pub.publish(mpc_stop_msg)
rospy.sleep(0.2)
self.__sendMpcTargetOdom([0.0, 0.0, 0.0], self.__mpc_horizon)
rospy.sleep(0.2)
mpc_stop_msg.data = False
self.__mpc_stop_flag_pub.publish(mpc_stop_msg)
rospy.loginfo("Start MPC control.")
rospy.sleep(0.2)
self.__mpc_start_odom = self.__hydrus_odom
self.__hydrus_motion_init_flag_pub.publish(Empty())
rospy.Timer(rospy.Duration(1.0 / self.__hydrus_controller_freq), self.__hydrusControllerCallback)
def on_wildfire_prediction(self, msg: PredictedWildfireMap):
if self.supervision_state == SupervisorNodeState.Propagation:
self.timer = rospy.Timer(rospy.Duration(secs=1, nsecs=0),
self.timer_callback,
oneshot=True)
def __init__(self):
# Flags for debugging and synchronization
self.print_robot_pose = False
self.have_map = False
self.map_token = False
self.map_compute = False
# Holds the occupancy grid map
self.ogm = 0
self.ogm_copy = 0
# Holds the ogm info for copying reasons -- do not change
self.ogm_info = 0
# Holds the robot's total path
self.robot_trajectory = []
# Holds the coverage information. This has the same size as the ogm
# If a cell has the value of 0 it is uncovered
# In the opposite case the cell's value will be 100
self.coverage = 0
# Holds the resolution of the occupancy grid map
self.resolution = 0.2
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
# Initialization of robot pose
# x,y are in meters
# x_px, y_px are in pixels
self.robot_pose = {}
self.robot_pose['x'] = 0
self.robot_pose['y'] = 0
self.robot_pose['th'] = 0
self.robot_pose['x_px'] = 0
self.robot_pose['y_px'] = 0
# Use tf to read the robot pose
self.listener = tf.TransformListener()
# Read robot pose with a timer
rospy.Timer(rospy.Duration(0.11), self.readRobotPose)
# ROS Subscriber to the occupancy grid map
ogm_topic = rospy.get_param('ogm_topic')
rospy.Subscriber(ogm_topic, OccupancyGrid, self.readMap)
# Publisher of the robot trajectory
robot_trajectory_topic = rospy.get_param('robot_trajectory_topic')
self.robot_trajectory_publisher = rospy.Publisher(robot_trajectory_topic,\
Path, queue_size = 10)
# Publisher of the coverage field
coverage_pub_topic = rospy.get_param('coverage_pub_topic')
self.coverage_publisher = rospy.Publisher(coverage_pub_topic, \
OccupancyGrid, queue_size = 10)
# Get the frames from the param file
self.map_frame = rospy.get_param('map_frame')
self.base_footprint_frame = rospy.get_param('base_footprint_frame')
import rospy
from std_msgs.msg import String
rospy.init_node('no2')
mat_string = String()
def subCallBack(msg):
global mat_string
mat_string = msg
def timerCallBack(event):
soma = 0
for i in range(len(mat_string.data)):
soma = soma + int(mat_string.data[i])
#print('somando matricula . . . ('+mat_string.data + ')')
msg_soma = String()
msg_soma.data = str(soma)
pub.publish(msg_soma)
sub = rospy.Subscriber('/no1/mat', String, subCallBack)
pub = rospy.Publisher('no2/sum', String, queue_size=1)
timer = rospy.Timer(rospy.Duration(1), timerCallBack)
rospy.spin()
def __init__(self):
self.debug_flag = True
self.arduino_flag = True
# Init Decoder
self.baudrate = rospy.get_param("~baudrate")
connect_success = False
while (connect_success == False):
# try to connect
try:
self.decoder = Decoder(self.baudrate)
self.arduino_decoder = ArduinoDecoder(19200)
connect_success = True
except Exception as e:
print e
rospy.sleep(0.1)
# Set decoder protocol
self.total_length = rospy.get_param("~total_length")
self.data_length = rospy.get_param("~data_length")
self.data_order = rospy.get_param("~data_order")
self.verify_switch = rospy.get_param("~verify_switch")
self.ccd_switch = rospy.get_param("~ccd_switch")
self.odo_switch = rospy.get_param("~odo_switch")
self.ack_codebook = {
"left_shoot": 1,
"right_shoot": 1,
"left_open": 1,
"right_open": 1,
"left_close": 0,
"right_close": 0
}
# offset buffer
self.x_offset = 0.0
self.y_offset = 0.0
self.theta_offset = 0.0
self.x_buf = 0.0
self.y_buf = 0.0
self.t_buf = 0.0
# setup frequency
self.frequency = 5.0
self.Rate = rospy.Rate(self.frequency)
self.plot_debug = True
self.display_init = False
# setup timer
self.start_time = rospy.Time.now()
self.select = 0
self.ccd_msg = CCD_data()
self.odo_buffer = [0.0, 0.0, 0.0]
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." % (self.node_name))
rospy.loginfo("[%s]: baudrate: [%s]" % (self.node_name, self.baudrate))
# Setup publishers
self.pub_odo_msg = rospy.Publisher("~odo_msg",
Pose2DStamped,
queue_size=1)
self.pub_odo_debug = rospy.Publisher("~odo_debug",
PoseStamped,
queue_size=1)
self.pub_ccd_msg = rospy.Publisher("~ccd_msg", CCD_data, queue_size=1)
self.pub_debug = rospy.Publisher("~debug", Joy, queue_size=1)
self.pub_ack_left_shoot = rospy.Publisher("~ack_left_shoot",
BoolStamped,
queue_size=1)
self.pub_ack_right_shoot = rospy.Publisher("~ack_right_shoot",
BoolStamped,
queue_size=1)
self.pub_ack_left_open = rospy.Publisher("~ack_left_open",
BoolStamped,
queue_size=1)
self.pub_ack_right_open = rospy.Publisher("~ack_right_open",
BoolStamped,
queue_size=1)
self.pub_delta_y = rospy.Publisher("~delta_y", Int32, queue_size=1)
# Setup Service
self.srv_offset = rospy.Service("~set_offset", Empty, self.cbSrvOffset)
self.lit = rospy.Service("~lit", Empty, self.cbLit)
self.die = rospy.Service("~die", Empty, self.cbDie)
# Setup serviceproxy
self.macro0 = rospy.ServiceProxy('/Robo/task_planner_node/macro0',
Empty)
self.macro1 = rospy.ServiceProxy('/Robo/task_planner_node/macro1',
Empty)
self.macro2 = rospy.ServiceProxy('/Robo/task_planner_node/macro2',
Empty)
# Read parameters
self.pub_timestep = self.setupParameter("~pub_timestep", 0.01)
# Create a timer that calls the process function every 1.0 second
self.debug_timer = rospy.Timer(
rospy.Duration.from_sec(self.pub_timestep), self.cbdebug)
#rospy.sleep(rospy.Duration.from_sec(1))
rospy.loginfo("[%s] Initialzed." % (self.node_name))
def __init__(self):
# Holds the current drone status.
self.status = -1
# Proivde services
# ----------------
# For now, the ``rospy.wait_for_service`` calls are
# disabled -- that way, this program will still run
# even if the drone isn't connected.
#
# `Toggle camera
# <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#toggle-camera>`_
toggle_camera = '/ardrone/togglecam'
#rospy.wait_for_service(toggle_camera)
self.ToggleCamera = rospy.ServiceProxy(toggle_camera, EmptyServiceType)
# Set camera channel (see link above).
set_camera_channel = '/ardrone/setcamchannel'
#rospy.wait_for_service(set_camera_channel)
self.SetCamera = rospy.ServiceProxy(set_camera_channel, CamSelect)
# `LED animations
# <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#led-animations>`_
led_animations = '/ardrone/setledanimation'
#rospy.wait_for_service(led_animations)
self.SetLedAnimation = rospy.ServiceProxy(led_animations, LedAnim)
# 'Flight animations
# <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#flight-animations>`_
# Be careful with these!
flight_animations = '/ardrone/setflightanimation'
#rospy.wait_for_service(flight_animations)
self.SetFlightAnimation = rospy.ServiceProxy(flight_animations,
FlightAnim)
# `Flat trim
# <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#flat-trim>`_
flat_trim = '/ardrone/flattrim'
#rospy.wait_for_service(flat_trim)
self.SetFlatTrim = rospy.ServiceProxy(flat_trim, EmptyServiceType)
# `Record to USB stick
# <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#record-to-usb-stick>`_
record_usb = '/ardrone/setrecord'
#rospy.wait_for_service(record_usb)
self.RecordUsb = rospy.ServiceProxy(record_usb, RecordEnable)
# Takeoff, land, and reset
# ------------------------
# Allow the controller to publish to the
# ``/ardrone/takeoff``, ``land`` and ``reset``
# topics.
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',
Empty,
queue_size=10)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=10)
# Velocity
# --------
# Allow the controller to publish to the
# ``/cmd_vel`` topic and thus control the drone.
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# Subscribe to the ``/ardrone/navdata`` topic, of
# message type navdata, and call
# ``self.ReceiveNavdata`` when a message is
# received.
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self._ReceiveNavdata)
# Set up regular publishing of control packets.
self.command = Twist()
self.commandTimer = rospy.Timer(
rospy.Duration(self.COMMAND_PERIOD / 1000.0), self._SendCommand)
# Shutdown
# --------
# Land the drone when we shut down.
rospy.on_shutdown(self.SendLand)
def cb_detection(self, msg):
# check there is more than 3 detections
# sort the detections by id
# check detection id 4 5 6 exist
if len(msg.detections) >= 3:
detections = sorted(msg.detections, key=lambda s: s.id[0])
id_check = 4
for i in range(3):
if i != detections[i].id[0]-4:
print('missing gate detection')
return
else:
id_check += 1
# calculate tags position relative to car
tag_poses = []
for i in range(3):
p = detections[i].pose.pose.pose.position
t = detections[i].pose.pose.pose.orientation
pos = [p.x, p.y, p.z]
tran = [t.x, t.y, t.z, t.w]
tag_tf = tf.transformations.concatenate_matrices(
tf.transformations.translation_matrix(pos), tf.transformations.quaternion_matrix(tran))
tag_position = np.dot(self.camera2foot, tag_tf)[0:3, 3]
tag_poses.append(tag_position)
tag_poses = np.array(tag_poses)
# ICP
total_tf = np.identity(4)
for i in range(self.iteration):
# find best transform in this iteration
transform = self.best_fit_transform(self.gate_poses, tag_poses)
# accumulate total transform
total_tf = np.dot(transform, total_tf)
# transform tag_poses
for i in range(tag_poses.shape[0]):
tag_poses[i] = transform[0:3, 3] + \
np.dot(transform[0:3, 0:3], tag_poses[i])
# transform matrix to quarternion
quat = tf.transformations.quaternion_from_matrix(total_tf)
quat = quat/np.linalg.norm(quat) # normalization
translation = total_tf[0:3, 3]
# save tf
if translation[0] < 0:
self.translations[self.count] = np.array(translation)
self.orientations[self.count] = np.array(quat)
self.count += 1
self.broadcaster.sendTransform(
translation, quat, rospy.Time.now(), "map", "tmp_global")
if self.count == MAX_DETECT:
# remove outlier using one class SVM
self.translations = self.remove_outlier(self.translations)
self.orientations = self.remove_outlier(self.orientations)
# calculate mean
self.trans_mean = np.mean(self.translations, axis=0)
self.orien_mean = np.mean(self.orientations, axis=0)
self.orien_mean = self.orien_mean / \
np.linalg.norm(self.orien_mean) # normalization
rospy.loginfo('%d translation is used' %
self.translations.shape[0])
rospy.loginfo('%d orientations is used' %
self.orientations.shape[0])
# stop subscriber
self.sub_detection.unregister()
# start timer to publish tf
self.tiemr = rospy.Timer(rospy.Duration(0.02), self.pub_tf)
def start_nodes_check_loop(self):
if self.__check_nodes_timer is None:
self.__check_nodes_timer = rospy.Timer(
rospy.Duration(1. / rospy.get_param("~check_nodes_frequency")),
self.__check_vital_nodes_callback)
pub.publish(msg)
msg.linear.x = default_v
msg.angular.z = move_w
pub.publish(msg)
def callback(event):
print("Rotated 10 sec using python")
rospy.signal_shutdown("Rotated 10 secs")
call = 0
createDaemon()
rospy.init_node('navigation_husky', anonymous=True)
if call == 0:
rospy.Timer(rospy.Duration(10), callback)
cmdmsg = geometry_msgs.msg.Twist()
cmdpub = rospy.Publisher('/cmd_vel',
geometry_msgs.msg.Twist,
queue_size=10)
rospy.Subscriber('odometry/filtered', nav_msgs.msg.Odometry,
huskyOdomCallback, (cmdpub, cmdmsg, call))
rospy.spin()
else:
cmdmsg = geometry_msgs.msg.Twist()
cmdpub = rospy.Publisher('/cmd_vel',
geometry_msgs.msg.Twist,
queue_size=10)
rospy.Subscriber('odometry/filtered', nav_msgs.msg.Odometry,
huskyOdomCallback, (cmdpub, cmdmsg, call))
rospy.signal_shutdown()
