def __init__(self):
serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
try:
self.device = BNO055(serial_port=serial_port)
except:
rospy.logerr('unable to find IMU at port {}'.format(serial_port))
sys.exit(-1)
if not self.device.begin():
rospy.logerr('unable to initialize IMU at port {}'.format(serial_port))
sys.exit(-1)
status = self.device.get_system_status()
rospy.loginfo('system status is {} {} {} '.format(*status))
time.sleep(1)
calibration_status = self.device.get_calibration_status()
rospy.loginfo('calibration status is {} {} {} {} '.format(*calibration_status))
self.device.set_external_crystal(True)
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1)
self.temp_pub = rospy.Publisher('temperature', Temperature, queue_size=1)
self.frame_id = rospy.get_param('~frame_id', '/base_imu')
self.seq = 0
self.reset_msgs()
def __init__(self, node_name):
ROS2OpenCV2.__init__(self, node_name)
self.match_threshold = rospy.get_param("~match_threshold", 0.7)
self.find_multiple_targets = rospy.get_param("~find_multiple_targets", False)
self.n_pyr = rospy.get_param("~n_pyr", 2)
self.min_template_size = rospy.get_param("~min_template_size", 25)
self.scale_factor = rospy.get_param("~scale_factor", 1.2)
self.scale_and_rotate = rospy.get_param("~scale_and_rotate", False)
self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
self.fov_width = rospy.get_param("~fov_width", 1.094)
self.fov_height = rospy.get_param("~fov_height", 1.094)
self.max_object_size = rospy.get_param("~max_object_size", 0.28)
# Intialize the detection box
self.detect_box = None
self.detector_loaded = False
rospy.loginfo("Waiting for video topics to become available...")
# Wait until the image topics are ready before starting
rospy.wait_for_message("input_rgb_image", Image)
if self.use_depth_for_detection:
rospy.wait_for_message("input_depth_image", Image)
rospy.loginfo("Ready.")
def __init__(self):
rospy.init_node('turtlebot_teleop')
self.l_scale = rospy.get_param('~drive_scale',0.6)
self.a_scale = rospy.get_param('~turn_scale',0.9)
self.deadman_button = rospy.get_param('~deadman_button', 0)
self.cmd = None
#publish cmd in Twist type to topic /cmd_vel
cmd_pub = rospy.Publisher('~cmd_vel', Twist)
# announce_pub = rospy.Publisher('/turtlebot_teleop/anounce/teleops',String, latch=True)
# anounce_pub.publisher(rospy.get_namespace());
rospy.Subscriber("joy", Joy, self.callback)
# rate = rospy.Rate(rospy.get_param('~hz', 20))
rate = rospy.Rate(10) #10hz
#keep sending cmd value while not shutdown
while not rospy.is_shutdown():
if self.cmd:
cmd_pub.publish(self.cmd)
rate.sleep()
def __init__(self):
message_class_str = rospy.get_param("~message",
"jsk_network_tools/FC2OCSLargeData")
try:
self.message_class = get_message_class(message_class_str)
except:
raise Exception("invalid topic type: %s"%message_class_str)
self.lock = Lock()
self.dynamic_reconfigure = Server(SilverhammerHighspeedStreamerConfig, self.dynamicReconfigureCallback)
self.launched_time = rospy.Time.now()
self.packet_interval = None
self.diagnostic_updater = diagnostic_updater.Updater()
self.diagnostic_updater.setHardwareID("none")
# register function to publish diagnostic
self.diagnostic_updater.add("HighspeedStreamer", self.diagnosticCallback)
self.send_port = rospy.get_param("~to_port", 16484)
self.send_ip = rospy.get_param("~to_ip", "localhost")
self.last_send_time = rospy.Time(0)
self.last_input_received_time = rospy.Time(0)
self.last_send_time_pub = rospy.Publisher("~last_send_time", Time)
self.last_input_received_time_pub = rospy.Publisher(
"~last_input_received_time", Time)
self.send_num = 0
self.rate = rospy.get_param("~send_rate", 2) #2Hz
self.socket_client = socket(AF_INET, SOCK_DGRAM)
self.packet_size = rospy.get_param("~packet_size", 1400) # for MTU:=1500
subscriber_info = subscribersFromMessage(self.message_class())
self.messages = {}
self.subscribe(subscriber_info)
self.counter = 0
self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate),
self.sendTimerCallback)
self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10),
self.diagnosticTimerCallback)
def __init__(self):
self.target_setpoint = Twist()
self.landed_setpoint = Twist()
self.flying = False
self.taking_off = False
self.landing = False
self.transient_height = 0
self.assisted_takeoff = rospy.get_param("~assisted_takeoff", True)
self.assisted_landing = rospy.get_param("~assisted_landing", True)
self.reconnect_on_takeoff = rospy.get_param("~reconnect_on_takeoff",
False)
self.goal_sub = rospy.Subscriber("goal", PoseStamped, self.new_goal)
self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
# Services
self.ts = rospy.Service('takeoff', Empty, self.takeoff)
self.ls = rospy.Service('land', Empty, self.land)
self._update_params = rospy.ServiceProxy('update_params', UpdateParams)
if self.reconnect_on_takeoff:
self._reconnect = rospy.ServiceProxy('reconnect', Empty)
# Send position setpoints 30 times per seconds
self.timer = rospy.Timer(rospy.Duration(1.0/UPDATE_RATE),
self.send_setpoint)
def controller(data):
if(data.data>0):
command = data.data
### вперед, назад, влево, вправо, поворот, вверх, вниз, медленнее, быстрее
if(command/100000==1): # ArDrone 2.0
if((command%1000)/100>0): #
go=True
while go==True:
params1=rospy.get_param("ardrone1_name")
params2=rospy.get_param("ardrone1_do")
params3=rospy.get_param("ardrone1_digit")
fb=params3[params1-1][0]-params3[params1-1][1]
rl=params3[params1-1][2]-params3 [params1-1][3]
ud=params3[params1-1][4]-params3[params1-1][5]
tt=params3[params1-1][6]
if(abs(fb)>0.0 or abs(rl)>0.0 or abs(ud)>0.0 or abs(tt)>0.0):
go=True
else:
go=False
pub3=rospy.Publisher('cmd_vel', Twist)
odom=Twist()
odom.linear.x=fb
odom.linear.y=rl
odom.linear.z=ud
odom.angular.x=0.0
odom.angular.y=0.0
odom.angular.z=tt
pub3.publish(odom)
rospy.loginfo("движение!!!!")
rospy.loginfo("конец движения!!!!")
elif(command/100000==2): # iRobot
rospy.loginfo("iRorot Robert")
def loadMap(self, pointset):
#pointset=str(sys.argv[1])
host = rospy.get_param("datacentre_host")
port = rospy.get_param("datacentre_port")
client = pymongo.MongoClient(host, port)
db=client.autonomous_patrolling
points_db=db["waypoints"]
available = points_db.find().distinct("meta.pointset")
#print available
if pointset not in available :
rospy.logerr("Desired pointset '"+pointset+"' not in datacentre")
rospy.logerr("Available pointsets: "+str(available))
raise Exception("Can't find waypoints.")
#points = self._get_points(waypoints_name)
points = []
search = {"meta.pointset": pointset}
for point in points_db.find(search) :
a= point["meta"]["name"]
b = topological_node(a)
b.edges = point["meta"]["edges"]
b.waypoint = point["meta"]["waypoint"]
b._get_coords()
b._insert_vertices(point["meta"]["vertices"])
points.append(b)
return points
def __init__(self):
super(ErrorNode, self).__init__()
self.DEBUG = bool(rospy.get_param('~DEBUG', 'False'))
# set topics
# self.ref_topic = rospy.get_param('~ref_topic')
# self.tgt_topic = rospy.get_param('~tgt_topic')
self.ref_tag = rospy.get_param('~ref_tag')
self.tgt_tag = rospy.get_param('~tgt_tag')
self.ref_topic = '/moos/'+self.ref_tag+'/odom'
self.tgt_topic = '/moos/'+self.tgt_tag+'/odom'
# self.pub_topic = rospy.get_param('~pub_topic')
self.pub_topic = '/error_mags/'+self.ref_tag+'_ref/'+self.tgt_tag+'_tgt'
# set pubs/subs
self.pub = rospy.Publisher(self.pub_topic, PoseError)
self.pub_test = rospy.Publisher('/test_error', Float64)
self.ref_sub = rospy.Subscriber(self.ref_topic, Odometry, self.onRefUpdate)
self.tgt_sub = rospy.Subscriber(self.tgt_topic, Odometry, self.onTgtUpdate)
# Data holders
self.ref_pose = Odometry()
self.tgt_pose = Odometry()
self.output = PoseError()
try:
self.sync_tol = rospy.get_param('~sync_tol')
except:
self.sync_tol = 0.15
def __init__(self, whicharm, tf_listener = None, wait_for_services = 1): #whicharm is 'right' or 'left'
#gets the robot_prefix from the parameter server. Default is pr2
robot_prefix = rospy.get_param('~robot_prefix', '')
self.srvroot = '/'+whicharm+'_arm_kinematics/'
#If collision_aware_ik is set to 0, then collision-aware IK is disabled
self.perception_running = rospy.get_param('~collision_aware_ik', 1)
self._ik_service = rospy.ServiceProxy(self.srvroot+'get_ik', GetPositionIK, True)
if self.perception_running:
self._ik_service_with_collision = rospy.ServiceProxy(self.srvroot+'get_constraint_aware_ik', GetConstraintAwarePositionIK, True)
self._fk_service = rospy.ServiceProxy(self.srvroot+'get_fk', GetPositionFK, True)
self._query_service = rospy.ServiceProxy(self.srvroot+'get_ik_solver_info', GetKinematicSolverInfo, True)
self._check_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity, True)
#wait for IK/FK/query services and get the joint names and limits
if wait_for_services:
self.check_services_and_get_ik_info()
if tf_listener == None:
rospy.loginfo("ik_utilities: starting up tf_listener")
self.tf_listener = tf.TransformListener()
else:
self.tf_listener = tf_listener
self.marker_pub = rospy.Publisher('interpolation_markers', Marker)
#dictionary for the possible kinematics error codes
self.error_code_dict = {} #codes are things like SUCCESS, NO_IK_SOLUTION
for element in dir(ArmNavigationErrorCodes):
if element[0].isupper():
self.error_code_dict[eval('ArmNavigationErrorCodes.'+element)] = element
#reads the start angles from the parameter server
start_angles_list = rospy.get_param('~ik_start_angles', [])
#good additional start angles to try for IK for the PR2, used
#if no start angles were provided
if start_angles_list == []:
self.start_angles_list = [[-0.697, 1.002, 0.021, -0.574, 0.286, -0.095, 1.699],
[-1.027, 0.996, 0.034, -0.333, -3.541, -0.892, 1.694],
[0.031, -0.124, -2.105, -1.145, -1.227, -1.191, 2.690],
[0.410, 0.319, -2.231, -0.839, -2.751, -1.763, 5.494],
[0.045, 0.859, 0.059, -0.781, -1.579, -0.891, 7.707],
[0.420, 0.759, 0.014, -1.099, -3.204, -1.907, 8.753],
[-0.504, 1.297, -1.857, -1.553, -4.453, -1.308, 9.572]]
else:
self.start_angles_list = start_angles_list
if whicharm == 'left':
for i in range(len(self.start_angles_list)):
for joint_ind in [0, 2, 4]:
self.start_angles_list[i][joint_ind] *= -1.
#changes the set of ids used to show the arrows every other call
self.pose_id_set = 0
rospy.loginfo("ik_utilities: done init")
def __init__(self):
node_name = rospy.get_name()
#print "node_name: " + str(node_name)
rate = rospy.get_param("~rate", 14.429)
self.history_size = rospy.get_param("~history_size", 5)
self.thresh = rospy.get_param("~thresh", 0.05)
try:
self.input_topic = rospy.get_param("~input_topic")
except:
err = 'Please specify an input topic'
rospy.logerr('Please specify an input topic')
raise Exception(err)
try:
self.output_topic = rospy.get_param("~output_topic")
except:
err = 'Please specify an output topic'
rospy.logerr(err)
raise Exception(err)
self.tf = tf.TransformListener()
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(rate)
self.history = RingBuffer(self.history_size)
self.parent = self.get_parent(self.input_topic)
def handler(self, respawn_request):
node_requesting = respawn_request.node_name
faulty_port = respawn_request.port_name
#Get both assigned port numbers:
control_port = rospy.get_param('AER_Driver/control_port')
sensor_port = rospy.get_param('Sensor_Monitor/sensor_port')
#Get a list of actually available ports
actual_ports = glob.glob('/dev/ttyACM*')
if (len(actual_ports) > 1):
#Handle the respawn request
available_ports = []
for port_number in actual_ports:
if port_number != sensor_port:
available_ports.append(port_number)
#Make sure we have a port available
if (len(available_ports) == 0):
#If not, send an error code
return RespawnResponse(2)
#If we have available ports
kill_cmd = "rosnode kill /" + node_requesting
spawn_cmd = "rosrun AER " + node_requesting + ".py&"
#Kill requesting node
subprocess.call(kill_cmd, shell=True)
#Set new parameter
param_name = 'AER_Driver/control_port'
rospy.set_param(param_name, available_ports[0])
#Rerun the node
subprocess.call(spawn_cmd, shell=True)
else:
#Send an error code
return RespawnResponse(1)
def talker():
rospy.init_node('random_motors', anonymous=False)
r = rospy.Rate(0.5) # 10hz
joints = [rospy.get_param("~pan_name", 'pan_joint'), rospy.get_param("~tilt_name", 'tilt_joint')]
torque_enable = dict()
servo_position = dict()
#Configure joints
for controller in sorted(joints):
try:
rospy.loginfo("controller is "+controller)
# Torque enable/disable control for each servo
torque_service = '/' + controller + '/torque_enable'
rospy.wait_for_service(torque_service)
torque_enable[controller] = rospy.ServiceProxy(torque_service, TorqueEnable)
# Start each servo in the disabled state so we can move them by hand
torque_enable[controller](False)
# The position controllers
servo_position[controller] = rospy.Publisher('/' + controller + '/command', Float64)
except:
rospy.logerr("Can't contact servo services!")
while not rospy.is_shutdown():
for controller in sorted(joints):
rand_val = uniform(-2,2)
#Send random float to each joint
servo_position[controller].publish(rand_val)
r.sleep()
"""
def __init__(self):
'''
Get file locations from parameter server (or use defaults) and setup dictionary
which specifies which values are updated.
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get file names from parameter server
self.file_urdf_in = rospy.get_param('~urdf_in', DEFAULT_CALIB_URDF_XACRO_IN)
self.file_urdf_out = rospy.get_param('~urdf_out', DEFAULT_CALIB_URDF_XACRO_OUT)
self.file_yaml_calib_system = rospy.get_param('~calib_system', DEFAULT_YAML_CALIB_SYSTEM)
self.file_yaml_init_system = rospy.get_param('~initial_system', DEFAULT_YAML_INITIAL_SYSTEM)
self.debug = rospy.get_param('~debug', ENABLE_DEBUG_OUTPUT)
# tfs2update stores the transform names [which need to be converted to (x, y, z, roll, pitch, yaw)
# and updated in the urdf] and their corresponding property name prefixes as used in calibration.urdf.xarco
self.tfs2update = {'arm_0_joint': 'arm_',
'torso_0_joint': 'torso_',
'head_color_camera_l_joint': 'cam_l_',
#'head_color_camera_r_joint': 'cam_r_',
'head_cam3d_rgb_optical_frame_joint': 'cam3d_'}
# chains2process stores the dh chain names of chains which need to be updated and their corresponding
# property names/segments as used in calibration.urdf.xarco
self.chains2update = {'arm_chain': ['arm_1_ref',
'arm_2_ref',
'arm_3_ref',
'arm_4_ref',
'arm_5_ref',
'arm_6_ref',
'arm_7_ref'],
'torso_chain': ['torso_lower_neck_tilt_cal_offset',
'torso_pan_cal_offset',
'torso_upper_neck_tilt_cal_offset']}
def _load_interface(self):
interface_file = resolve_url(rospy.get_param('~interface_url', ''))
if interface_file:
rospy.loginfo("interface_url: %s", interface_file)
try:
data = read_interface(interface_file) if interface_file else {}
# set the ignore hosts list
self._re_ignore_hosts = create_pattern('ignore_hosts', data, interface_file, [])
# set the sync hosts list
self._re_sync_hosts = create_pattern('sync_hosts', data, interface_file, [])
self.__sync_topics_on_demand = False
if interface_file:
if 'sync_topics_on_demand' in data:
self.__sync_topics_on_demand = data['sync_topics_on_demand']
elif rospy.has_param('~sync_topics_on_demand'):
self.__sync_topics_on_demand = rospy.get_param('~sync_topics_on_demand')
rospy.loginfo("sync_topics_on_demand: %s", self.__sync_topics_on_demand)
self.__resync_on_reconnect = rospy.get_param('~resync_on_reconnect', True)
rospy.loginfo("resync_on_reconnect: %s", self.__resync_on_reconnect)
self.__resync_on_reconnect_timeout = rospy.get_param('~resync_on_reconnect_timeout', 0)
rospy.loginfo("resync_on_reconnect_timeout: %s", self.__resync_on_reconnect_timeout)
except:
import traceback
# kill the ros node, to notify the user about the error
rospy.logerr("Error on load interface: %s", traceback.format_exc())
import os
import signal
os.kill(os.getpid(), signal.SIGKILL)
def __init__(self):
rospy.init_node('receiver')
PACKAGE = rospy.get_param('~package')
NAME = rospy.get_param('~message_type')
TOPIC = rospy.get_param('~topic_name')
PORT = rospy.get_param('~port_number')
MESSAGE = getattr(__import__(PACKAGE, fromlist=[NAME]), NAME)
receiver_pub = rospy.Publisher(TOPIC, MESSAGE, queue_size=10)
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(("", PORT))
sock.listen(1)
sock, addr = sock.accept()
except Exception as e:
sock.close()
rospy.loginfo("RECEIVER ERROR:")
rospy.loginfo(e)
sys.exit()
while True:
compressed_message = self.recv_msg(sock)
message = zlib.decompress(compressed_message)
if not message is None:
depickled_message = pickle.loads(message)
topic_message = depickled_message
receiver_pub.publish(topic_message)
def __init__(self):
#publishing rate
self.rate = 30 #30Hz
node_name = 'imu_mpu9150_handler'
rospy.init_node(node_name)
rospy.loginfo(rospy.get_caller_id() + 'Initialising {} node'.format(node_name))
# Get all parameters from config (rosparam)
address = int(rospy.get_param('imu/address', 0x69))
gyro_scale = int(rospy.get_param('imu/gyroscope/scale', 250))
acc_scale = int(rospy.get_param('imu/accelerometer/scale', 2))
# Initialize Madgwick Algorithm
#self.madgwick = MadgwickAHRS(1/30, 2.7101, 0.2);
self.quaternion = Madgwick(0.041, self.rate)
rospy.loginfo('Test {} address'.format(address))
self.imu = Mpu9150(address, gyro_scale, acc_scale)
#self.imu = Mpu9250(gyro_scale, acc_scale)
self.imu.calibrate();
rospy.loginfo("Compass calibration; move the IMU in random positions during the calibration");
(self.mag_offset, self.mag_scale) = self.imu.magCalibration();
rospy.loginfo("Calibration done");
def main():
rospy.init_node('exploration')
# CONFIG
# robot
x, y = rospy.get_param('~start_position', [2.5, 2.5])
z = rospy.get_param('~flight_height', 1.199)
simulation.configure('start_position', [x, y, z])
rot = rospy.get_param('~start_orientation', 0.)
simulation.configure('start_rotation', rot)
# world
w, h = rospy.get_param('~world_size', [10., 10.])
simulation.configure('world_width', w)
simulation.configure('world_height', h)
vsize = rospy.get_param('~voxel_size', .1)
simulation.configure('voxel_size', vsize)
# spawn simulation thread
rospy.loginfo('starting computation of exploration path')
SimWorker().start()
# register service
rospy.loginfo('registering waypoint service')
s = rospy.Service('get_waypoint', GetWaypoint, handle_get_waypoint)
# don't leave
rospy.spin()
def start_recognizer(self):
rospy.loginfo("Starting recognizer... ")
self.pipeline = gst.parse_launch(self.launch_config)
self.asr = self.pipeline.get_by_name('asr')
self.asr.connect('partial_result', self.asr_partial_result)
self.asr.connect('result', self.asr_result)
self.asr.set_property('configured', True)
self.asr.set_property('dsratio', 1)
# Configure language model
if rospy.has_param(self._lm_param):
lm = rospy.get_param(self._lm_param)
else:
rospy.logerr('Recognizer not started. Please specify a language model file.')
return
if rospy.has_param(self._dic_param):
dic = rospy.get_param(self._dic_param)
else:
rospy.logerr('Recognizer not started. Please specify a dictionary.')
return
self.asr.set_property('lm', lm)
self.asr.set_property('dict', dic)
self.bus = self.pipeline.get_bus()
self.bus.add_signal_watch()
self.bus_id = self.bus.connect('message::application', self.application_message)
self.pipeline.set_state(gst.STATE_PLAYING)
self.started = True
def __init__(self):
#start node
rospy.init_node("map_saver")
rospy.on_shutdown(self.die)
#get parameters from server
self.save_dir = rospy.get_param("map_save_dir","map_log/") #default to .ros/map_log
update_rate = rospy.get_param("map_save_update_rate",120) #update once every 2 min
#create directory and set clean_open
self.clean_open=self.create_dir()
#create dictionaries
self.edges={} #dictionary of edge_id:edge_obj
self.nodes={} #dictionary of node_id:node_obj
#begin listening
rospy.Subscriber("/node",GVGNode, self.node_handler)
rospy.Subscriber("/edge",GVGEdgeMsg, self.edge_handler)
self.updated=False
#while alive and able to open the file
self.alive=True
while(self.clean_open and self.alive):
if(self.updated):
self.dump(str(rospy.get_time()))
self.updated=False
rospy.sleep(update_rate)
def __init__(self, use_dummy_transform=False):
rospy.init_node('star_center_positioning_node')
if use_dummy_transform:
self.odom_frame_name = ROBOT_NAME+"_odom_dummy"
else:
self.odom_frame_name = ROBOT_NAME+"_odom"
self.marker_locators = {}
self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi))
self.pose_correction = rospy.get_param('~pose_correction',0.0)
self.phase_offset = rospy.get_param('~phase_offset',0.0)
self.is_flipped = rospy.get_param('~is_flipped',False)
srv = Server(STARPoseConfig, self.config_callback)
self.marker_sub = rospy.Subscriber("ar_pose_marker",
ARMarkers,
self.process_markers)
self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10)
self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10)
self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
def talker():
global joy_value
global last_joy
rospy.init_node('vrep_ros_teleop')
sub = rospy.Subscriber('~joy', Joy, joy_cb)
pub = rospy.Publisher('~twistCommand', Twist, queue_size=1)
axis_linear = rospy.get_param("~axis_linear",1)
axis_angular = rospy.get_param("~axis_angular",0)
scale_linear = rospy.get_param("~scale_linear",5.)
scale_angular = rospy.get_param("~scale_angular",10.)
timeout = True
while not rospy.is_shutdown():
twist = Twist()
if (rospy.rostime.get_time() - last_joy) < 0.5:
if timeout:
timeout = False
rospy.loginfo("Accepting joystick commands")
twist.linear.x = joy_value.axes[axis_linear] * scale_linear
twist.angular.z = joy_value.axes[axis_angular] * scale_angular
if twist.linear.x < 0:
twist.angular.z = - twist.angular.z
else:
if not timeout:
timeout = True
rospy.loginfo("Timeout: ignoring joystick commands")
pub.publish(twist)
rospy.sleep(0.1)
def execute(self, userdata):
self.__offset = rospy.get_param('youbot_manipulation_ai_re/misc/angle_offset')
self.__adjust_arm_threshold = rospy.get_param('youbot_manipulation_ai_re/misc/adjust_arm_threshold')
self.__image_timeout = rospy.get_param('youbot_manipulation_ai_re/misc/image_timeout')
self.__wait_for_image_loop_rate = rospy.get_param('youbot_manipulation_ai_re/misc/wait_for_image_loop_rate')
c_height = userdata.camera_params_in['height']
msg_joint = copy.deepcopy(userdata.joint_position_in)
while(1):
t1 = rospy.Time.now().to_sec()
obj = None
r = rospy.Rate(self.__wait_for_image_loop_rate)
while(1):
obj = self.get_object(userdata.selected_object_name_in)
t2 = rospy.Time.now().to_sec()
if t2 - t1 > self.__image_timeout:
return 'object lost'
if obj != None:
break
r.sleep()
if abs(obj.rrect.centerPoint.y + obj.rrect.height/2.0 - c_height) < self.__adjust_arm_threshold:
userdata.joint_position_out = copy.deepcopy(msg_joint)
break
time.sleep(0.5)
angle = global_data.ik.get_joint_values()[4] - self.__offset
#x = -0.001 * (cos(angle) - sin(angle))
#y = -0.001 * (sin(angle) + cos(angle))
x = 0.001 * cos(angle)
y = 0.001 * sin(angle)
msg_joint['x'] += x
msg_joint['y'] += y
global_data.ik.change_pos(msg_joint)
return 'arm adjusted'
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
self._SerialPublisher = rospy.Publisher('serial', String)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom", Odometry)
self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")
self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)
self._State = Arduino.CONTROLLER_RESET_REQUIRED
self._SerialDataGateway = SerialDataGateway(port, baudRate, self._HandleReceivedLine)
def __init__(self):
rospy.init_node("max_sonar")
self.sensor = maxSonar()
# start channel broadcast using SetupAnalogIn
rospy.wait_for_service('arbotix/SetupAnalogIn')
analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel)
req = SetupChannelRequest()
req.topic_name = rospy.get_param("~name")
req.pin = rospy.get_param("~pin")
req.rate = int(rospy.get_param("~rate",10))
analog_srv(req)
# setup a range message to use
self.msg = Range()
self.msg.radiation_type = self.sensor.radiation_type
self.msg.field_of_view = self.sensor.field_of_view
self.msg.min_range = self.sensor.min_range
self.msg.max_range = self.sensor.max_range
# publish/subscribe
self.pub = rospy.Publisher("sonar_range", Range)
rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
rospy.spin()
def __init__(self):
# Get topic class
topic_class = None
rospy.loginfo("Waiting for topic to become available...")
while not rospy.is_shutdown() and topic_class == None:
topic_class, topic, _ = rostopic.get_topic_class(rospy.resolve_name('in'))
rospy.sleep(0.5)
if hasattr(topic_class, 'header'):
cb = self.header_cb
elif topic_class == tf2_msgs.msg.TFMessage:
cb = self.tf_cb
self.frames = set(rospy.get_param('~frames'))
self.frame_prefix = rospy.get_param('~frame_prefix')
else:
print("Message type {} doesn't have a header field.".format(topic_class))
sys.exit(1)
self.start_time = None
self.actual_start_time = None
self.time_offset = None
self.sub = rospy.Subscriber('in', topic_class, cb)
self.pub = rospy.Publisher('out', topic_class, queue_size=1)
def __init__(self):
rospy.init_node("velo_gripper_controller")
self.speed = rospy.get_param("~speed", 0.5)
self.torque = rospy.get_param("~torque", 500)
self.min_opening = rospy.get_param("~min", 0.0)
self.max_opening = rospy.get_param("~max", 2.0)
self.controller = 'velo_controller'
if self.torque > 1023:
self.torque = 1023
if self.torque < 0:
self.torque = 0
#torque_service = '/' + self.controller + '/torque_enable'
torque_service = '/' + self.controller + '/set_torque_limit'
# torque_service = '/' + self.controller + '/set_compliance_punch' # min torque
# torque_service = '/' + self.controller + '/set_compliance_limit' # max torque
# limit, punch 0-1023
rospy.wait_for_service(torque_service)
#self.my_torque_service = rospy.ServiceProxy
rospy.wait_for_service(torque_service)
#self.my_torque_service = rospy.ServiceProxy (torque_service, TorqueEnable)
self.my_torque_service = rospy.ServiceProxy (torque_service, SetTorqueLimit)
speed_service = '/' + self.controller + '/set_speed'
rospy.wait_for_service(speed_service)
# speed_services.append(rospy.ServiceProxy (speed_service, SetSpeed))
self.my_speed_service = rospy.ServiceProxy (speed_service, SetSpeed)
# position in radians
pub_topic = '/' + self.controller + '/command'
self.velo_pub = rospy.Publisher(pub_topic, Float64)
# subscribe to command and then spin
# Pr2GripperCommand(position, max_effort)
self.server = actionlib.SimpleActionServer(side_c +"_gripper_controller/gripper_action", Pr2GripperCommandAction, execute_cb=self.commandCb, auto_start=False)
self.server.start()
def __init__(self):
self.id = rospy.get_param("~id")
self.prefix = rospy.get_param("~prefix")
self.gps = None
self.state = None
self.mission = None
self.battery = None
self.rel_alt = 0
self.ready = False
self.type = Droid.MAV_TYPE.unknown
self.droid_known_data = {}
self.goto_once = False
self.mode = Droid.MODE.wait
self.mode_wait = Droid.MAV_MODE.guided
self.mode_data = {}
# Comunication Topic
self.topic_comm_pub = rospy.Publisher( Droid.TOPIC_COMM, DroidInfo, queue_size=10 )
rospy.Subscriber( Droid.TOPIC_COMM, DroidInfo, self.handler_topic_comm )
# Droid services
rospy.Service( Droid.SERVICE_WAIT, Empty, self.handler_service_wait )
rospy.Service( Droid.SERVICE_AUTO, Empty, self.handler_service_auto )
rospy.Service( Droid.SERVICE_FOLLOW, DroidFollow, self.handler_service_follow )
rospy.Service( Droid.SERVICE_GOTO, DroidGoto, self.handler_service_goto )
rospy.Service( Droid.SERVICE_MISSION, DroidMission, self.handler_service_mission )
# Mavros data
rospy.Subscriber( Droid.TOPIC_GPS, NavSatFix, self.handler_topic_gps )
rospy.Subscriber( Droid.TOPIC_STATE, State, self.handler_topic_state )
rospy.Subscriber( Droid.TOPIC_MISSION_STATE, WaypointList, self.handler_topic_mission )
rospy.Subscriber( Droid.TOPIC_ALT, Float64, self.handler_topic_alt )
rospy.Subscriber( Droid.TOPIC_BATTERY, BatteryStatus, self.handler_topic_battery )
# Mavros topic
self.topic_rc_pub = rospy.Publisher( Droid.TOPIC_RC_OVERRIDE, OverrideRCIn, queue_size = 10 )
# Mavros service
rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_GOTO )
self.service_mav_goto = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_GOTO, WaypointGOTO )
rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_PUSH )
self.service_mav_mission_push = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_PUSH, WaypointPush )
rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_PULL )
self.service_mav_mission_pull = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_PULL, WaypointPull )
rospy.wait_for_service( Droid.SERVICE_MAV_MODE )
self.service_mav_mode = rospy.ServiceProxy( Droid.SERVICE_MAV_MODE, SetMode )
rospy.wait_for_service( Droid.SERVICE_MAV_ARM )
self.service_mav_arm = rospy.ServiceProxy( Droid.SERVICE_MAV_ARM, CommandBool )
rospy.wait_for_service( Droid.SERVICE_MAV_PARAM_SET )
self.service_mav_param_set = rospy.ServiceProxy( Droid.SERVICE_MAV_PARAM_SET, ParamSet )
rospy.wait_for_service( Droid.SERVICE_MAV_PARAM_GET )
self.service_mav_param_get = rospy.ServiceProxy( Droid.SERVICE_MAV_PARAM_GET, ParamGet )
def launch_control_operator_node():
rospy.init_node('dronemap_control_operator', anonymous=True)
# get parameters for the client
platform_ip_address = rospy.get_param('~cloud_platform_ip_address')
platform_port_number = rospy.get_param('~cloud_platform_port_number')
buffer_size = rospy.get_param('~buffer_size')
# set global parameters for other processes (eg. hearbeat_operator_node)
rospy.set_param('/cloud_platform_ip_address', platform_ip_address)
rospy.set_param('/cloud_platform_port_number', platform_port_number)
rospy.set_param('/buffer_size', buffer_size)
rospy.set_param('/drone_registered', False)
# get the formatter
formatter = create_message_formatter()
# start the operator
client = TCPClient("dronemap_control_operator", platform_ip_address, platform_port_number, formatter, buffer_size)
DronemapControlOperator(client)
# Keep python from exiting until this node is stopped
rospy.spin()
def save_all_data_params(self):
"""
Save parameters to data file for future reference.
"""
experiment_path = rospy.get_param("/faa_experiment/experiment_path")
params = rospy.get_param("/")
self.write_yaml_file(os.path.join(experiment_path, "params.yaml"), params)
def __init__(self):
rospy.init_node('twist_converter', anonymous=True)
self.num_robots=int(rospy.get_param('~num_robots',1))
self.publishers = [None]*self.num_robots;
self.subscribers = [None]*self.num_robots;
if rospy.has_param('~robot_prefix'): #if there is a robot prefix assume that there is actually one or more
#full_param_name = rospy.search_param('robot_prefix')
#robot_prefix = rospy.get_param(full_param_name)
robot_prefix=rospy.get_param('~robot_prefix')
for r in range(self.num_robots):
self.publishers[r]=rospy.Publisher(robot_prefix+str(r)+'/cmd_vel',Twist);
self.subscribers[r] = rospy.Subscriber(robot_prefix+str(r)+'/image', Image, self.callback, r)
else: # if no robot prefix, assume that there is only one robot
self.publishers[0] = rospy.Publisher('cmd_vel',Twist);rospy.logwarn("assuming /cmd_vel, number of robots actually "
+str(self.num_robots))
self.subscribers[0] = rospy.Subscriber("image", Image, self.callback, 0)
self.data_uri = rospy.get_param("data_uri","/twist");
self.urls = (self.data_uri,'twist', "/stop","stop","/controller","controller")
self.data = ['-10']*self.num_robots;
self.datasize = [0]*self.num_robots;
self.port=int(rospy.get_param("~port","8080"));
#self.data_uri2 = rospy.get_param("data_uri","/pose");
rospy.logwarn("running")
def _post_init(self):
if not rospy.get_param('~lazy', True):
self.subscribe()
self._connection_status = True
def load_table():
table_bounds = map(float, rospy.get_param("table_bounds").split())
kinbodies.create_box_from_bounds(Globals.pr2.env,
table_bounds,
name="table")
def setup():
if Globals.pr2 is None:
Globals.pr2 = PR2.PR2.create(rave_only=args.test)
if not args.test: load_table()
if Globals.rviz is None: Globals.rviz = ros_utils.RvizWrapper.create()
Globals.table_height = rospy.get_param("table_height")
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
# How big is the square we want the robot to navigate?
square_size = rospy.get_param("~square_size", 1.0) # meters
# Create a list to hold the waypoint poses
waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
#First Round
waypoints.append(
Pose(Point(3.26524591446, 0.372401684523, 0.000),
Quaternion(0.000, 0.000, 0.0, 1.0)))
waypoints.append(
Pose(Point(4.18248939514, 1.63796591759, 0.000),
Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
waypoints.append(
Pose(Point(1.63975059986, 2.0675611496, 0.000),
Quaternion(0.000, 0.000, -0.707106781187, 0.707106781187)))
waypoints.append(
Pose(Point(1.4075371027, 1.04582130909, 0.000),
Quaternion(0.000, 0.000, 0.0, 1.0)))
#Second Round
waypoints.append(
Pose(Point(4.18248939514, 1.63796591759, 0.000),
Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
waypoints.append(
Pose(Point(1.4075371027, 1.04582130909, 0.000),
Quaternion(0.000, 0.000, 0.0, 1.0)))
#Third Round
waypoints.append(
Pose(Point(1.63975059986, 2.0675611496, 0.000),
Quaternion(0.000, 0.000, -0.707106781187, 0.707106781187)))
waypoints.append(
Pose(Point(4.18248939514, 1.63796591759, 0.000),
Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
waypoints.append(
Pose(Point(3.26524591446, 0.372401684523, 0.000),
Quaternion(0.000, 0.000, 0.0, 1.0)))
waypoints.append(
Pose(Point(1.4075371027, 1.04582130909, 0.000),
Quaternion(0.000, 0.000, 0.0, 1.0)))
#Last Ending Point
waypoints.append(
Pose(Point(0.299762457609, 0.106840103865, 0.000),
Quaternion(0.000, 0.000, 0.701216898681, 0.712948007223)))
#waypoints.append(Pose(Point(1.759, -1.282,0.000), quaternions[2]))
#waypoints.append(Pose(Point(-0.442, -0.039, 0.000), quaternions[3]))
#waypoints.append(Pose(Point(1.759, -1.282,0.000), quaternions[4]))
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < 11 and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *
rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)
tc.WaitForAuto()
try:
tc.Wander(front_sector=True)
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
if not rospy.core.is_shutdown():
tc.SetManual()
rospy.loginfo("Mission completed")
def __init__(self):
print "Initializing Launchpad Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel',Int64,queue_size = 10)
self._Right_Encoder = rospy.Publisher('rwheel',Int64,queue_size = 10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',Float32,queue_size = 10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',Float32,queue_size = 10)
#Publisher for IMU rotation quaternion values
self._qx_ = rospy.Publisher('qx',Float32,queue_size = 10)
self._qy_ = rospy.Publisher('qy',Float32,queue_size = 10)
self._qz_ = rospy.Publisher('qz',Float32,queue_size = 10)
self._qw_ = rospy.Publisher('qw',Float32,queue_size = 10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
#######################################################################################################################
#Subscribers and Publishers of IMU data topic
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer =[]
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu,queue_size = 10)
self.deltat = 0
self.lastUpdate = 0
#New addon for computing quaternion
self.pi = 3.14159
self.GyroMeasError = float(self.pi * ( 40 / 180 ))
self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)
self.GyroMeasDrift = float(self.pi * ( 2 / 180 ))
self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)
self.beta = math.sqrt(3 / 4) * self.GyroMeasError
self.q = [1,0,0,0]
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed',Float32,self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',Float32,self._Update_Right_Speed)
def __init__(self, root):
rospy.init_node('tello_ui', anonymous=False)
# rospy.Subscriber('/command_pos', Point, self.command_pos_callback)
signal.signal(signal.SIGINT, self.onClose)
signal.signal(signal.SIGTERM, self.onClose)
try:
self.id = rospy.get_param('~ID')
except KeyError:
self.id = 0
self.publish_prefix = "tello{}/".format(self.id)
self.point_command_pos = Point(0.0, 0.0, 1.0)
self.point_command_pos_yaw = 0.0
self.command_pos = Pose()
self.rotated_pos = Point()
self.slam_pos = Point()
self.twist_manual_control = Twist()
self.real_world_pos = Point()
self.delta_pos = Point()
self.orientation_degree = Point()
self.allow_slam_control = False
self.current_mux = 0
# initialize the root window and image panel
self.root = root
self.panel = None
# self.panel_for_pose_handle_show = None
# self.client = dynamic_reconfigure.client.Client("orb_slam2_mono")
# create buttons
self.column = 0
self.row = 0
self.frame_column = 0
self.frame_row = 0
self.angle_delta_x = 0
self.angle_delta_y = 0
self.angle_delta_z = 0
self.angle = 12.0
self.angle_radian = self.angle / 180.0 * math.pi
self.real_world_scale = 3.9636
self.altitude = 0
self.init_command_pos_frame_flag = False
self.init_real_world_frame_flag = False
self.init_slam_pose_frame_flag = False
self.init_delta_frame_flag = False
self.init_speed_frame_flag = False
self.init_info_frame_flag = False
self.init_manual_control_frame_flag = False
self.init_angle_calc_frame_flag = False
self.init_rotated_frame_flag = False
self.init_command_pos_frame()
self.init_real_world_frame()
self.init_rotated_frame()
# self.init_slam_pose_frame()
self.init_delta_frame()
self.init_speed_frame()
self.init_info_frame()
self.init_manual_control_frame()
# self.init_angle_calc_frame()
self.init_kd_kp_frame()
self.update_command_pos_to_gui()
self.row += 1
self.column = 0
# set a callback to handle when the window is closed
self.root.wm_title("TELLO Controller")
self.root.wm_protocol("WM_DELETE_WINDOW", self.onClose)
self.kd = Pose()
self.kp = Pose()
rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slam_callback)
rospy.Subscriber(self.publish_prefix+'delta_pos', Point, self.delta_pos_callback)
rospy.Subscriber(self.publish_prefix+'cmd_vel', Twist, self.speed_callback)
rospy.Subscriber(self.publish_prefix+'flight_data', FlightData, self.flightdata_callback)
rospy.Subscriber(self.publish_prefix+'allow_slam_control', Bool, self.allow_slam_control_callback)
rospy.Subscriber(self.publish_prefix+'real_world_scale', Float32, self.real_world_scale_callback)
rospy.Subscriber(self.publish_prefix+'real_world_pos', PoseStamped, self.real_world_pos_callback)
rospy.Subscriber(self.publish_prefix+'rotated_pos', Point, self.rotated_pos_callback)
rospy.Subscriber(self.publish_prefix+'command_pos', Pose, self.command_pos_callback)
rospy.Subscriber(self.publish_prefix+'orientation', Point, self.orientation_callback)
self.command_pos_publisher = rospy.Publisher(self.publish_prefix+'command_pos', Pose, queue_size = 1)
self.pub_takeoff = rospy.Publisher(self.publish_prefix+'takeoff', Empty, queue_size=1)
self.pub_land = rospy.Publisher(self.publish_prefix+'land', Empty, queue_size=1)
self.pub_allow_slam_control = rospy.Publisher(self.publish_prefix+'allow_slam_control', Bool, queue_size=1)
self.cmd_val_publisher = rospy.Publisher(self.publish_prefix+'cmd_vel', Twist, queue_size = 1)
self.calibrate_real_world_scale_publisher = rospy.Publisher(self.publish_prefix+'calibrate_real_world_scale', Empty, queue_size = 1)
self.scan_room_publisher = rospy.Publisher(self.publish_prefix+'scan_room', Bool, queue_size = 1)
self.kd_publisher = rospy.Publisher(self.publish_prefix+'kd', Pose, queue_size = 1)
self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
self.pub_mux = rospy.Publisher('tello_mux', Int32, queue_size = 1)
self.publish_command()
state_filter.header = header
state_filter.states = (be,b,om,al,v,a)
self.fcp.publish(line_coeffs3)
self.fpp.publish(points_coeffs3_pcl)
self.sfp.publish(state_filter)
def was_called(self):
return self.called
if __name__ == '__main__':
rospy.init_node('filter')
### LOADING PARAMETERS ###
cmd_vel_topic = rospy.get_param('topics/cmd_vel')
line_coeffs_topic = rospy.get_param('topics/line_coeffs')
filtered_line_coeffs_topic = rospy.get_param('topics/filtered_line_coeffs')
filtered_line_pcl_topic = rospy.get_param('topics/filtered_line_pcl')
state_filter_topic = rospy.get_param('topics/state_filter')
base_link = rospy.get_param('tfs/base_link')
l = rospy.get_param('l')
verbose = rospy.get_param('kalman/verbose')
q00 = rospy.get_param('kalman/q00')
q11 = rospy.get_param('kalman/q11')
q22 = rospy.get_param('kalman/q22')
q33 = rospy.get_param('kalman/q33')
return cont, q_sol.flatten()
if __name__ == "__main__":
load_file = True
if load_file:
dirName = os.path.split(os.path.abspath(os.path.realpath(sys.argv[0])))[0] + '/casadi_urdf'
# fileName = "%s/centauro_urdf_6dof_joints_1111110.txt" % dirName
fileName = "%s/centauro_urdf_7dof_joints_1111110_torso.txt" % dirName
with open(fileName, 'r') as f:
urdf = f.read()
# print urdf
else:
urdf = rospy.get_param('robot_description')
kindyn = cas_kin_dyn.CasadiKinDyn(urdf)
end_effector = 'arm1_8'
forKin = Function.deserialize(kindyn.fk(end_effector))
joints = config.JointNames('torso')
joints.addJoints('arm1')
# joints = config.JointNames('arm1')
joint_str = joints.getName()
joint_num = [1,2,3,4,5,6]
for joint in joint_str:
try:
if int(joint[-1]) not in joint_num:
joint_str.remove(joint)
except:
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
# How big is the square we want the robot to navigate?
square_size = rospy.get_param("~square_size", 1.0) # meters
# Create a list to hold the waypoint poses
waypoints = list()
# Create the waypoints and add them to the list
self.createWaypoints()
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 10 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(10))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting turtlebot navigation")
# Initialize a counter to track waypoints
i = 0
# Go through every waypoint and
for waypoint in waypoints:
# Update the marker display
self.marker_pub.publish(self.markers)
# Initialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move_turtlebot(goal)
# Increment i to get the next waypoint on the next loop.
i = i + 1
def __init__(self):
# 파라미터 설정
self.lin_vel_joy = rospy.get_param('~lin_vel_joy', 0.69)
self.ang_vel_joy = rospy.get_param('~ang_vel_joy', 3.67)
self.camera = rospy.get_param('~camera', 'camera/color/image_raw')
self.spin_cycle = rospy.Duration(rospy.get_param('~spin_cycle', 0.05))
self.scale_arrow = rospy.get_param('~scale_arrow', 50)
self.scale_cross = rospy.get_param('~scale_cross', 30)
# 화면 초기화
os.environ['SDL_VIDEO_WINDOW_POS'] = "0, 0"
pygame.init()
self.monitor = pygame.display.Info()
self.width = rospy.get_param('~width',
int(0.48 * self.monitor.current_w))
self.height = rospy.get_param('~height',
int(0.48 * self.monitor.current_h))
self.screen = pygame.display.set_mode((self.width, self.height))
pygame.mouse.set_visible(False)
pygame.display.set_caption("Shared control interface")
# 토픽 구독
print(C_YELLO + '\rInterfacer, BCI 서비스 준비중...' + C_END)
rospy.Subscriber(self.camera, Image, self.visualize)
self.color = {
'data': [
(255, 223, 36), # default
(255, 223, 36), # M_RIGHT
(255, 223, 36), # M_LEFT
(255, 223, 36), # M_FORWARD
(255, 223, 36),
(255, 223, 36),
(255, 223, 36),
(134, 229, 127)
], # M_MOVE
'time': [rospy.get_time()] * 8
}
# 출력 설정
self.publisher_cmd_intuit = rospy.Publisher('interf/cmd/intuit',
CmdIntuit,
queue_size=1)
self.publisher_cmd_assist = rospy.Publisher('interf/cmd/assist',
CmdAssist,
queue_size=1)
self.publisher_nav_cue = rospy.Publisher('interf/nav_cue',
NavCue,
queue_size=1)
self.publisher_cmd_vel = rospy.Publisher('cmd_vel',
Twist,
queue_size=1)
self.publisher_cmd_joint = rospy.Publisher('cmd_joint',
JointState,
queue_size=1)
# 토픽 구독
self.cmd = CmdIntuit()
self.switch_marker = [False, False, False]
rospy.Subscriber('interf/cmd/intuit', CmdIntuit,
self.update_cmd_intuit)
rospy.Subscriber('interf/robot/motion', RobotMotion,
self.update_marker_color)
rospy.Subscriber('interf/nav_cue', NavCue,
self.update_marker_visibility)
rospy.Subscriber('joy', Joy, self.joystick)
self.path = []
rospy.Subscriber('robot/pose', PoseWithCovarianceStamped,
self.update_robot_pose)
# 서비스 시작
self.publisher_time_start = rospy.Publisher('time/start',
Time,
queue_size=1)
self.publisher_time_end = rospy.Publisher('time/end',
Time,
queue_size=1)
self.time_start = rospy.Time.now()
self.the_timer = rospy.Timer(rospy.Duration(0.1), self.timer)
self.path_publisher = rospy.Publisher('interf/path',
MarkerArray,
queue_size=1)
self.path_visualizer = rospy.Timer(rospy.Duration(0.3),
self.visualize_path)
rospy.Service('interf/nav2cmd', Nav2Cmd, self.nav2cmd)
self.key_watcher = rospy.Timer(self.spin_cycle, self.keyboard)
print(C_YELLO + '\rInterfacer, BCI 서비스 시작' + C_END)
print(C_GREEN + '\rInterfacer, 초기화 완료' + C_END)
sts = 0
rospy.loginfo("Running Mapper")
try:
rospy.init_node('mapper')
map_path = ""
waypoint_file = ""
lm_button_index = -1
ms_button_index = -1
if sts == 0:
if map_path == "":
map_path = rospy.get_param("~map_path","")
rospy.loginfo("Found param map_path='%s'" % map_path)
map_path = map_path.strip()
if map_path == "":
rospy.loginfo("Error: No map_path specified")
print usage()
sts = 1
else:
rospy.loginfo("Map and waypoints will be saved in: '%s'" % map_path)
if sts ==0:
if waypoint_file == "":
waypoint_file = rospy.get_param("~waypoint_file","")
rospy.loginfo("Found param Waypointfile='%s'" % waypoint_file)
from controller import Robot
from joint_state_publisher import JointStatePublisher
from trajectory_follower import TrajectoryFollower
# from gripper_control import GripperFollower
from rosgraph_msgs.msg import Clock
parser = argparse.ArgumentParser()
parser.add_argument('--node-name',
dest='nodeName',
default='Panda',
help='Specifies the name of the node.')
arguments, unknown = parser.parse_known_args()
rospy.init_node(arguments.nodeName, disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
# initialPosition = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'Panda' else ''
# for i in range(len(initialPosition)):
# name = 'panda_joint'+str(i+1)
# robot.getMotor(name).setPosition(initialPosition[i])
jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, nodeName)
trajectoryFollower.start()
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def __init__(self, verbose=False):
self.mc = rospy.get_param("~mc", False)
self.num_particles = rospy.get_param("~num_particles", 100)
if verbose:
print(self)
def __init__(self):
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("roboclaw_node_pitch")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
self.dev_name = rospy.get_param("~dev", "/dev/ttyACM3") #may need to change the usb port
self.baud_rate = int(rospy.get_param("~baud", "38400")) #may need to change the baud rate. see roboclaw usermanual
self.address = int(rospy.get_param("~address", "128")) #roboclaw is current setup to use address 128 which is setting 7
#Error Handle
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
roboclaw.Open(self.dev_name, self.baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.ResetEncoders(self.address)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "127"))
self.MAX_DUTY = float(rospy.get_param("~max_duty", "32767"))
self.last_set_speed_time = rospy.get_rostime()
self.last_pub_time = rospy.get_rostime()
self.pitch_vel_pub = rospy.Subscriber("roboclaw/pitch_vel", Twist, self.cmd_vel_callback)
self.pitch_pub = rospy.Publisher("roboclaw/pitch", JointState, queue_size=10)
self.m1_duty = 0
self.m2_duty = 0
rospy.sleep(1)
rospy.loginfo("dev %s", self.dev_name)
rospy.loginfo("baud %d", self.baud_rate)
rospy.loginfo("address %d", self.address)
rospy.loginfo("max_speed %f", self.MAX_SPEED)
def __init__(self):
# Load a mission and parse the file
mission_file = rospy.get_param('~mission_file')
deep_motion_planner = rospy.get_param('~deep_motion_planner',
default=False)
stage_simulation = rospy.get_param('~stage_simulation', default=True)
if not os.path.exists(mission_file):
rospy.logerr('Mission file not found: {}'.format(mission_file))
exit()
self.mission = MissionFileParser(mission_file).get_mission()
self.mission_index = 0 # Currently executed command
self.random_waypoint_number = 0 # Number of random waypoints remaining
self.current_target = [0, 0, 0] # Current goal pose
self.command_start = rospy.Time.now().to_sec(
) # Time when command execution started
self.command_timeout = rospy.get_param('~command_timeout',
default=360.0)
self.costmap = None # Cache for the costmap
self.tf_broadcaster = tf.TransformBroadcaster()
# ROS topics
self.start_pub = rospy.Publisher('/start', Empty, queue_size=1)
self.stop_pub = rospy.Publisher('/stop', Empty, queue_size=1)
self.abort_pub = rospy.Publisher('/abort', Empty, queue_size=1)
self.target_pub = rospy.Publisher('/relative_target',
PoseStamped,
queue_size=1)
self.costmap_sub = rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, \
self.__costmap_callback__)
self.costmap_update_sub = rospy.Subscriber('/move_base/global_costmap/costmap_updates', \
OccupancyGridUpdate, self.__costmap_update_callback__)
self.joystick_sub = rospy.Subscriber('/joy', Joy,
self.joystick_callback)
if deep_motion_planner:
self.navigation_client = actionlib.SimpleActionClient(
'deep_move_base', MoveBaseAction)
while not self.navigation_client.wait_for_server(
rospy.Duration(5)):
rospy.loginfo('Waiting for deep planner action server')
else:
# connect to the action api of the move_base package
self.navigation_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
while not self.navigation_client.wait_for_server(
rospy.Duration(5)):
rospy.loginfo('Waiting for move_base action server')
if stage_simulation:
rospy.wait_for_service('/reset_positions')
# Start the mission if it is not empty
if len(self.mission) > 0:
rospy.loginfo('Start mission')
self.__send_next_command__()
else:
rospy.logerr('Mission file contains no commands')
rospy.signal_shutdown('Mission Finished')
def update_cmd_pwm(self, msg):
pin = int(msg.pin)
# Gère les messages entre -100 et 100
if np.abs(msg.command)>100.0:
msg.command = 0.0
self.out_tab[pin].publish(msg.command/2.0*10+1500)
if __name__ == '__main__':
rospy.init_node('simu_pwm_board')
# === COMMON ===
# La node doit se lancer en sachant où chercher sa config
node_name = rospy.get_name()
device_type_name = rospy.get_param(node_name+'_type_name')
# Config
ns = rospy.get_namespace()
nslen = len(ns)
prefix_len = nslen + 5 # On enlève le namespace et simu_
config_node = rospy.get_param('robot/'+device_type_name+'/'+node_name[prefix_len:])
print config_node
# Pas sur qu'on en ai besoin mais au cas où
device_type = config_node['type']
config_device = rospy.get_param('device_types/'+device_type)
print config_device
# === SPECIFIC ===
def loadParameters(self):
self.droneList = rospy.get_param('~droneList', ['cf2', 'cf3'])
self.NumDrones = len(self.droneList)
pass
self._right_params.camera_name,
result_msg.right_result)
rospy.loginfo("Finished Calibrating")
self._calibrator = None
#rospy.signal_shutdown('Finished')
if __name__ == '__main__':
rospy.init_node("stereo_calibrator")
rospy.loginfo("Loading %s" % rospy.get_name())
# Get parameters
left_params = CalibratorParams()
right_params = CalibratorParams()
autostart = rospy.get_param("~autostart", False)
use_service = rospy.get_param("~use_service", True)
approximate = rospy.get_param("~approximate", 0.0)
left_params.features = "%s/features" % rospy.remap_name("left")
left_params.camera_info = "%s/camera_info" % rospy.remap_name("left")
left_params.service_name = "%s/set_camera_info" % rospy.remap_name(
"left_camera")
left_params.camera_name = rospy.get_param("~left_camera_name", "left")
left_params.file_name = rospy.get_param("~left_file_name", "left.yaml")
right_params.features = "%s/features" % rospy.remap_name("right")
right_params.camera_info = "%s/camera_info" % rospy.remap_name("right")
right_params.service_name = "%s/set_camera_info" % rospy.remap_name(
"right_camera")
right_params.camera_name = rospy.get_param("~right_camera_name", "right")
from std_msgs.msg import ColorRGBA
# Shows Center of mass and support polygon in rviz
rospy.init_node('markers_stability', anonymous=False)
pub_markers = rospy.Publisher('/markers/stability',
Marker,
queue_size=1,
latch=True)
rospy.loginfo("Markers Stability started.")
rospy.loginfo("Waiting for COM radius parameter...")
param_name = "/robostilt/safety/center_of_mass_radius"
while (rospy.has_param(param_name) is False):
rospy.sleep(1.0)
COM_radius = rospy.get_param(param_name)
rospy.loginfo("Markers Stability publishing...")
namespace = "stabilty"
# holds the different markers
indexes = ("com", "com_projected", "support_polygon")
markers = []
# initialize markers
for i in range(0, len(indexes)):
markers.append(Marker())
markers[i].id = i
markers[i].ns = namespace
markers[i].action = Marker.ADD
def __init__(self):
rospy.init_node('dbw_node')
# Inputs
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.1
self.twist_cmd_sub = rospy.Subscriber(
"/twist_cmd",
TwistStamped,
self.twist_cmd_handler
)
self.dbw_enabled_sub = rospy.Subscriber(
"/vehicle/dbw_enabled",
Bool,
self.dbw_enabled_handler
)
self.current_velocity_sub = rospy.Subscriber(
"/current_velocity",
TwistStamped,
self.current_velocity_handler
)
# State
self.initialized = False
self.dbw_enabled = False
self.current_velocity = None
self.twist_cmd = None
self.controller = Controller(
vehicle_mass=vehicle_mass,
fuel_capacity=fuel_capacity,
brake_deadband=brake_deadband,
decel_limit=decel_limit,
accel_limit=accel_limit,
wheel_radius=wheel_radius,
wheel_base=wheel_base,
steer_ratio=steer_ratio,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle,
min_speed=min_speed,
)
# Outputs
self.steering_cmd_pub = rospy.Publisher(
'/vehicle/steering_cmd',
SteeringCmd,
queue_size=1
)
self.throttle_cmd_pub = rospy.Publisher(
'/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1
)
self.brake_cmd_pub = rospy.Publisher(
'/vehicle/brake_cmd',
BrakeCmd,
queue_size=1
)
self.loop()
def cb_control(self, msg_control):
self.updateParameters()
rospy.loginfo(rospy.get_param('/control/manual'))
rospy.loginfo(rospy.get_param('/control/step'))
self.control.pid.set_setpoint(self.setpoint)
from std_msgs.msg import Bool
from race.msg import drive_param
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Int32
'''Need rospy and message types for eStop (bool), drive_parameters (drive_param), scan (LaserScan)'''
from math import radians, degrees, pi #for conversions
from math import * #for conversions
from numpy import isnan
import numpy
#global parameters
'''Drive parameters'''
velocity = rospy.get_param("/initial_speed", "12")
SIDE = rospy.get_param("/initial_side", -1)
#SIDE = 1 is right SIDE = -1 is left
'''Publisher'''
#em_pub = rospy.Publisher('eStop', Bool, queue_size=10)
v_pub = rospy.Publisher('drive_velocity', Int32, queue_size=1)
activated = False
# 12 --> 1
# 30 --> 2
# 45 --> 5.5
### 5.5 is good enough but 6 for extra safety
FRONT_BUMPER_THRESHOLD = 3.0
print 'geting parameter from dynamic reconfigure...'
isStart = config['isStart']
isAbort= config['isAbort']
search_depth=config['search_depth']
search_distance=config['search_distance']
z_threshold =config['z_threshold']
ver_dist_to_pinger=config['altitude_at_pinger']-0.6+0.13
#1.13: dist from hydrophone to DVL, 0.5, height of pinger
return config
if __name__ == '__main__':
rospy.init_node('acoustic_navigation',log_level=rospy.DEBUG)
loop_rate=rospy.Rate(0.5)
rospy.loginfo("AcousticNavigation activated!")
rospy.wait_for_service('set_controller_srv')
isTest = rospy.get_param('~testmode',False)
#Setup dynamic reconfigure
reconfigure_srv=Server(acoustic_navConfig, dynamic_reconfigure_cb)
#Setup movement client
movement_client = actionlib.SimpleActionClient('LocomotionServer', bbauv_msgs.msg.ControllerAction)
movement_client.wait_for_server()
if isTest:
isStart= True
cur_heading=10000 #dummy val
set_controller_request=rospy.ServiceProxy('set_controller_srv',set_controller)
set_controller_request(True,True,True,True,False,False,False)
else:
def init_hardware_test(self):
rospy.init_node('hardware_test')
### INTERNAL PARAMETERS
self.wait_time = 5 # waiting time (in seconds) before trying initialization again
self.wait_time_recover = 1
self.wait_time_diag = 3
###
### GET PARAMETERS ###
# Get actuator parameters
try:
params_base = rospy.get_param('/hardware_test/components/base')
for k in params_bases.keys():
self.base_params.append(params_base[k])
except:
pass
# Get actuator parameters
try:
params_actuator = rospy.get_param('/hardware_test/components/actuators')
for k in params_actuator.keys():
self.actuators.append(params_actuator[k])
except:
pass
# Get sensor parameters
try:
params = rospy.get_param('/hardware_test/components/sensors')
for k in params.keys():
self.sensors.append(params[k])
except:
pass
if not self.base_params and not self.actuators and not self.sensors:
raise NameError('Couldn\'t find any components to test under /component_test namespace. '
'You need to define at least one component in order to run the program (base, actuator, sensor). '
'View the documentation to see how to define test-components.')
# Get log-file directory
try:
log_dir = rospy.get_param('/hardware_test/result_dir')
except:
log_dir = '/tmp'
# Create and prepare logfile
self.complete_name = '%s/hardware_test_%s.txt' %(log_dir, time.strftime("%Y%m%d_%H-%M"))
self.log_file = open(self.complete_name,'w')
self.log_file.write('Robotino Hardware test')
self.log_file.write('\n%s \n%s' %(time.strftime('%d.%m.%Y'), time.strftime('%H:%M:%S')))
self.print_topic('TESTED COMPONENTS')
if self.actuators:
for actuator in self.actuators:
self.log_file.write('\n ' + actuator['name'])
self.print_topic('TEST PARAMETERS')
params = rospy.get_param('/hardware_test/components')
for key in params:
self.log_file.write('\n%s:' %(key))
params_2 = rospy.get_param('/hardware_test/components/%s' %(key))
for key_2 in params_2:
self.log_file.write('\n %s:' %(key_2))
params_3 = rospy.get_param('/hardware_test/components/%s/%s' %(key, key_2))
for key_3, value in params_3.iteritems():
self.log_file.write('\n %s: %s' %(key_3, value))
self.print_topic('!!!!!!!!!!!!!!!!!!!!TEST LOG!!!!!!!!!!!!!!!!!!!!')
def inverted_palm_regrasp(axis, angle, velocity):
with open(
"/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml",
'r') as stream:
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
pose_target = group.get_current_pose().pose
pos_initial = [
pose_target.position.x, pose_target.position.y, pose_target.position.z
]
ori_initial = [
pose_target.orientation.x, pose_target.orientation.y,
pose_target.orientation.z, pose_target.orientation.w
]
T_we = tf.TransformListener().fromTranslationRotation(
pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, config['object_thickness'] / 2, 0,
1] #TODO: depends on axis direction
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG
# Interpolate orientation poses via quaternion slerp
q = helper.axis_angle2quaternion(axis, angle)
ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
ori_waypoints = helper.slerp(
ori_initial, ori_target,
np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))
theta_0 = config['theta_0']
waypoints = []
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
for psi in range(1, angle + 1):
# Calculate width
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
# Calculate position
if theta_0 + psi <= 90:
hori = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi)))
verti = math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi))) - math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi)))
else:
hori = -math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi - 90)))
verti = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi - 90)))
if axis[0] > 0:
pose_target.position.y = contact_A_w[
1] - hori # TODO: The only thing I changed for inverted palm regrasp is the (-) sign
pose_target.position.z = contact_A_w[2] - verti
#print "CASE 1"
elif axis[0] < 0:
pose_target.position.y = contact_A_w[1] + hori
pose_target.position.z = contact_A_w[2] - verti
#print "CASE 2"
elif axis[1] > 0:
pose_target.position.x = contact_A_w[0] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 3"
elif axis[1] < 0:
pose_target.position.x = contact_A_w[0] + hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 4"
pose_target.orientation.x = ori_waypoints[psi - 1][0]
pose_target.orientation.y = ori_waypoints[psi - 1][1]
pose_target.orientation.z = ori_waypoints[psi - 1][2]
pose_target.orientation.w = ori_waypoints[psi - 1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
retimed_plan = group.retime_trajectory(robot.get_current_state(), plan,
velocity)
group.execute(retimed_plan, wait=False)
opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
psi = 0
while psi < angle:
pose = group.get_current_pose().pose
q_current = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
psi = 2 * math.degrees(math.acos(np.dot(q_current, ori_initial)))
if psi > 100:
psi = -(psi - 360)
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
palm_position = 127 + (config['delta_0'] - a) * 1000
#pos = int((opening_at_zero - width)/config['opening_per_count'])
Robotiq.goto(robotiq_client,
pos=width + 0.003,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
dynamixel.set_length(palm_position + 9)
psi = round(psi, 2)
def updateParameters(self):
self.setParameters(rospy.get_param('/control/parameters'))
self.setStepParameters(rospy.get_param('/control/step'))
self.setManualParameters(rospy.get_param('/control/manual'))
self.setAutoParameters(rospy.get_param('/control/automatic'))
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.INFO)
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
# Distance from front tires to rear tires
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
# Ratio between turn of steering wheel and turn of wheel
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd, queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd, queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd, queue_size=1)
# TODO: Create `Controller` object
self.controller = Controller(
vehicle_mass,
wheel_radius,
accel_limit,
decel_limit,
wheel_base,
steer_ratio,
0,
max_lat_accel,
max_steer_angle)
# TODO: Subscribe to all the topics you need to
# Made up of a Header (with a time stamp, and frame_id), and Twist
# (linear and angular velocity vectors)
rospy.Subscriber('/twist_cmd', TwistStamped, self.proposed_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_cb)
# Simulator Publishes wheter or not the drive by wire is enabled (only
# publish if it is)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.proposed_linear_vel = 0.
self.proposed_angular_vel = 0.
# self.proposed_time = 0.
self.current_linear_vel = 0.
self.current_anuglar_vel = 0.
# obtain current time from /twist_cmd.header.nsecs (or secs)
# the time stamp in /current_velocity is empty
self._time_nsecs = None
self.dbw_enabled = False
if WRITE_CSV_LOG:
# NOTE: this file is created at /home/<user>/.ros/log/
self._logfile = open("log.csv", "w")
fieldnames = ["time_nsecs",
"current_linear_vel",
"proposed_angular_vel",
"proposed_linear_vel",
"throttle", "brake", "steering"]
self._logwriter = csv.DictWriter(self._logfile,
fieldnames=fieldnames)
self._logwriter.writeheader()
self.loop()
def active_regrasp(axis, angle, velocity, active_distance,
psi_active_transition, active_distance_2):
with open(
"/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml",
'r') as stream:
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
pose_target = group.get_current_pose().pose
pos_initial = [
pose_target.position.x, pose_target.position.y, pose_target.position.z
]
ori_initial = [
pose_target.orientation.x, pose_target.orientation.y,
pose_target.orientation.z, pose_target.orientation.w
]
T_we = tf.TransformListener().fromTranslationRotation(
pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
error = 0.00
contact_A_e = [
tcp2fingertip + random.uniform(-error, error),
-config['object_thickness'] / 2 + random.uniform(-error, error), 0, 1
] #TODO: depends on axis direction
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG
# Interpolate orientation poses via quaternion slerp
q = helper.axis_angle2quaternion(axis, angle)
ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
ori_waypoints = helper.slerp(
ori_initial, ori_target,
np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))
theta_0 = config['theta_0']
waypoints = []
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name,
CommandRobotiqGripperAction)
for psi in range(1, angle + 1):
# Calculate width
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
# Calculate position
if theta_0 + psi <= 90:
hori = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi)))
verti = math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi))) - math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi)))
else:
hori = -math.fabs(tcp2fingertip * math.sin(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.cos(math.radians(theta_0 + psi - 90)))
verti = math.fabs(tcp2fingertip * math.cos(
math.radians(theta_0 + psi - 90))) + math.fabs(
(width / 2.0) * math.sin(math.radians(theta_0 + psi - 90)))
if axis[0] > 0:
pose_target.position.y = contact_A_w[1] + hori
pose_target.position.z = contact_A_w[2] + verti
print "CASE 1"
elif axis[0] < 0:
if psi <= psi_active_transition:
pose_target.position.y = contact_A_w[
1] - hori - active_distance * psi / psi_active_transition
else:
pose_target.position.y = contact_A_w[
1] - hori - active_distance + active_distance_2 * (
psi - psi_active_transition) / (angle + 1 -
psi_active_transition)
pose_target.position.z = contact_A_w[2] + verti
print "CASE 2"
elif axis[1] > 0:
pose_target.position.x = contact_A_w[0] - hori
pose_target.position.z = contact_A_w[2] + verti
print "CASE 3"
elif axis[1] < 0:
pose_target.position.x = contact_A_w[0] + hori
pose_target.position.z = contact_A_w[2] + verti
print "CASE 4"
pose_target.orientation.x = ori_waypoints[psi - 1][0]
pose_target.orientation.y = ori_waypoints[psi - 1][1]
pose_target.orientation.z = ori_waypoints[psi - 1][2]
pose_target.orientation.w = ori_waypoints[psi - 1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
retimed_plan = group.retime_trajectory(robot.get_current_state(), plan,
velocity)
group.execute(retimed_plan, wait=False)
opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
psi = 0
while psi < angle:
pose = group.get_current_pose().pose
q_current = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
psi = 2 * math.degrees(math.acos(np.dot(q_current, ori_initial)))
if psi > 100:
psi = -(psi - 360)
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
#pos = int((opening_at_zero - width)/config['opening_per_count'])
Robotiq.goto(
robotiq_client,
pos=width + 0.000,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False) #0.006 for coin; 0.000 for book; 0.005 for poker
#if psi < 1.0 or psi > 47.0:
#print "psi= ", psi, " width= ", width
psi = round(psi, 2)
rospy.sleep(0.5) # TESTING TO SEE IF THE GRIPPER ACTION DOESNT LAG
return width
