def cartesian_direct(self):
rospy.loginfo(rospy.get_caller_id() + ' -> starting cartesian direct')
self.prepare_cartesian()
# set in position cartesian mode
self.set_state_block('DVRK_POSITION_CARTESIAN')
# get current position
initial_cartesian_position = self._position_cartesian_desired
goal = Pose()
# create a new goal starting with current position
goal.position.x = initial_cartesian_position.position.x
goal.position.y = initial_cartesian_position.position.y
goal.position.z = initial_cartesian_position.position.z
goal.orientation.x = initial_cartesian_position.orientation.x
goal.orientation.y = initial_cartesian_position.orientation.y
goal.orientation.z = initial_cartesian_position.orientation.z
goal.orientation.w = initial_cartesian_position.orientation.w
# motion parameters
amplitude = 0.05 # 5 cm
duration = 5 # 5 seconds
rate = 200 # aiming for 200 Hz
samples = duration * rate
for i in xrange(samples):
goal.position.x = initial_cartesian_position.position.x + amplitude * math.sin(i * math.radians(360.0) / samples)
goal.position.y = initial_cartesian_position.position.y + amplitude * math.sin(i * math.radians(360.0) / samples)
self.set_position_cartesian.publish(goal)
errorX = goal.position.x - self._position_cartesian_desired.position.x
errorY = goal.position.y - self._position_cartesian_desired.position.y
errorZ = goal.position.z - self._position_cartesian_desired.position.z
error = math.sqrt(errorX * errorX + errorY * errorY + errorZ * errorZ)
if error > 0.002: # 2 mm
print 'Inverse kinematic error in position [', i, ']: ', error
rospy.sleep(1.0 / rate)
rospy.loginfo(rospy.get_caller_id() + ' <- cartesian direct complete')
def callback(data):
print rospy.get_caller_id(), "I heard [%s], %d"%(data.time, len(data.joint_states))
for j in data.joint_states:
print " joint: %s" % j.name
print " position: %.4f" % j.position
print " velocity: %.4f" % j.velocity
print " applied_effort: %.4f" % j.applied_effort
print " commanded_effort: %.4f" % j.commanded_effort
for a in data.actuator_states:
print " actuator: %s" % a.name
print " encoder_count: %d" % a.encoder_count
print " position: %d" % a.position
print " timestamp: %.4f" % a.timestamp
print " encoder_velocity: %.4f" % a.encoder_velocity
print " velocity: %.4f" % a.velocity
print " calibration_reading: %d" % a.calibration_reading
print " last_calibration_high_transition: %d" % a.last_calibration_high_transition
print " last_calibration_low_transition: %d" % a.last_calibration_low_transition
print " is_enabled: %d" % a.is_enabled
print " run_stop_hit: %d" % a.run_stop_hit
print " last_requested_current: %.4f" % a.last_requested_current
print " last_commanded_current: %.4f" % a.last_commanded_current
print " last_measured_current: %.4f" % a.last_measured_current
print " last_requested_effort: %.4f" % a.last_requested_effort
print " last_commanded_effort: %.4f" % a.last_commanded_effort
print " last_measured_effort: %.4f" % a.last_measured_effort
print " motor_voltage: %.4f" % a.motor_voltage
print " num_encoder_errors: %d" % a.num_encoder_errors
def spi_data(data):
if (data.spi_cmd == 24):
rospy.loginfo(rospy.get_caller_id() + " SPI : X")
if (data.spi_cmd == 40):
rospy.loginfo(rospy.get_caller_id() + " SPI : A")
if (data.spi_cmd == 56):
rospy.loginfo(rospy.get_caller_id() + " SPI : B")
def __init__(self, cloud_platform_client):
self.cloud_client = cloud_platform_client
rospy.loginfo(rospy.get_caller_id() + ": Prepare & send Registration Request")
# Prepare a Registration Request message
request = RegistrationRequestMessage()
request.messageType = 'RegistrationRequestMessage'
#get information from the status manager interface... implementation db or paramaeters or in-memory
# information are 1-drone type
# 2-battery level
# 3- gps coords
# 4 -serial number
# s il te plais avant d implementet tout ca, test avec un simple example et verfier s interagit avec multiple callbacks
# Send the Registration Request message & receive the Registration Response message
response = self.cloud_client.send(request, True)
if response == None or response.status == 0:
rospy.loginfo(rospy.get_caller_id() + ": Registration Request refused")
sys.exit() # Launch SystemExit exception, so it can be intercepted at an outler level
rospy.set_param("/drone_registered", True)
rospy.set_param("/cloud_session_id", response.sessionID)
rospy.set_param("/cloud_drone_id", response.UAVID)
rospy.loginfo(rospy.get_caller_id() + ": Registration Request accepted")
def bump_bag_record(data):
global count
global t
global recorded
print ("Motor State Value : %s " % (data))
if(data.state == 0 and count == 0):
print ("BAZINGA BAZINGA BAZINGA")
count= count + 1
rospy.loginfo(rospy.get_caller_id()+"Start Recording!")
global procMessage
t = time.time()
recorded = True
procMessage = subprocess.Popen(["rosbag", "record", "-a","-O", startValue], preexec_fn=os.setsid)
#time.sleep(10)
elif(data.state == 0 and not count == 0 and not count == 10):
global count
count = count + 1
elif(count == 10):
global count
count = 0
elif((time.time() - t) >= 10 and recorded == True):
t = 0
recorded = False
rospy.loginfo(rospy.get_caller_id()+"STOPPING RECORDING")
if 'procMessage' in globals():
rospy.loginfo(rospy.get_caller_id()+"Killing Rosbag")
terminate_ros_node("/record")
print count, recorded
def execute_cb(self, goal):
global is_moving
is_moving=True
# helper variables
#r = rospy.Rate(1)
success = True
# publish info to the console for the user
rospy.loginfo('%s: Executing, creating JointTrajectory with %i points' % (self._action_name, len(goal.trajectory.points)))
# start executing the action
for i in range(len(goal.trajectory.points)):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
rospy.loginfo(rospy.get_caller_id()+"Trajectory point %i: %s", i,goal.trajectory.points[i].positions)
self.sendJointState(goal.trajectory.points[i])
rospy.loginfo(rospy.get_caller_id()+" Done")
#self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
#self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
#r.sleep()
if success:
#self._result.status = True
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
is_moving=False
def cartesian_goal(self):
rospy.loginfo(rospy.get_caller_id() + ' -> starting cartesian goal')
self.prepare_cartesian()
# set in position cartesian mode
self.set_state_block('DVRK_POSITION_GOAL_CARTESIAN')
# get current position
initial_cartesian_position = self._position_cartesian_desired
goal = Pose()
# create a new goal starting with current position
goal.position.x = initial_cartesian_position.position.x
goal.position.y = initial_cartesian_position.position.y
goal.position.z = initial_cartesian_position.position.z
goal.orientation.x = initial_cartesian_position.orientation.x
goal.orientation.y = initial_cartesian_position.orientation.y
goal.orientation.z = initial_cartesian_position.orientation.z
goal.orientation.w = initial_cartesian_position.orientation.w
# motion parameters
amplitude = 0.05 # 5 cm
# first motion
goal.position.x = initial_cartesian_position.position.x - amplitude
goal.position.y = initial_cartesian_position.position.y + amplitude
self.set_position_goal_cartesian_publish_and_wait(goal)
# second motion
goal.position.x = initial_cartesian_position.position.x + amplitude
goal.position.y = initial_cartesian_position.position.y - amplitude
self.set_position_goal_cartesian_publish_and_wait(goal)
# back to initial position
goal.position.x = initial_cartesian_position.position.x
goal.position.y = initial_cartesian_position.position.y
self.set_position_goal_cartesian_publish_and_wait(goal)
rospy.loginfo(rospy.get_caller_id() + ' <- cartesian goal complete')
def left_arm():
Curr_pos = 0 ;
rate = rospy.Rate(50) # 50hz
timer = 0;
forw = 1;
time.sleep(1);
flag = 1;
while (flag == 1 and rospy.is_shutdown() == 0):
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to left arm 1" ,Curr_pos );
RA1.publish(1.0 );
time.sleep(1);
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to left arm 2" ,Curr_pos );
LA2.publish(-1.57 );
time.sleep(1);
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to left arm 3" ,Curr_pos );
LA3.publish(Curr_pos );
time.sleep(1);
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to left arm 4" ,Curr_pos );
LA4.publish(Curr_pos );
time.sleep(1);
flag = 0;
def button(data):
if (str(data.button_x) == 'True'):
rospy.loginfo(rospy.get_caller_id() + " X TRI")
if (str(data.button_a) == 'True'):
rospy.loginfo(rospy.get_caller_id() + " A CENT")
if (str(data.button_b) == 'True'):
rospy.loginfo(rospy.get_caller_id() + " B SNIP")
def callback(data):
global currentIndex
global waitingTime
global frameId
if data.data:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
else:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
currentIndex = (currentIndex+1) % len(waypoints)
# Waiting time before sending next goal
rospy.sleep(waitingTime)
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
except ValueError:
is_dig = False
if is_dig:
msg.node_id = int(waypoints[currentIndex])
msg.node_label = ""
else:
msg.node_id = 0
msg.node_label = waypoints[currentIndex]
rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
msg.header.stamp = rospy.get_rostime()
pub.publish(msg)
def run(self):
global FORMAT, CHUNK_SIZE, RATE
num_retries = 30
time.sleep(5) # try to prevent crash when starting at boot
self.p = pyaudio.PyAudio()
# attempt to connect to the audio stream. on startup, this may
# initially fail, so retry a few times
for num_tries in range(num_retries):
try:
rospy.loginfo(rospy.get_caller_id() + ": starting audio listener for input device " + str(self.input_device) + " try " + str(num_tries + 1))
self.stream = None
self.stream = self.p.open(format=FORMAT, channels=1, rate=RATE, input=True,
frames_per_buffer=CHUNK_SIZE, input_device_index=self.input_device)
break
except:
traceback.print_exc()
time.sleep(1)
if self.stream is None:
rospy.loginfo(rospy.get_caller_id() + ": failed to start audio listener for input device " + str(self.input_device) + " after " + str(num_retries) + " tries")
exit(1)
while not self.is_shutdown:
# determine speech segment from input audio
# store last speech segment recorded while listening
self.last_segment = Segment(*self.record())
# close audio stream on exit
rospy.loginfo(rospy.get_caller_id() + ": stopping audio listener")
self.stream.stop_stream()
self.stream.close()
self.p.terminate()
rospy.loginfo(rospy.get_caller_id() + ": audio listener terminated")
return
def joint_goal(self):
rospy.loginfo(rospy.get_caller_id() + " -> starting joint goal")
# set in position joint mode
self.set_state_block("DVRK_POSITION_GOAL_JOINT")
# get current position
initial_joint_position = []
initial_joint_position[:] = self._position_joint_desired
rospy.loginfo(
rospy.get_caller_id() + " -> testing goal joint position for 2 joints of %i", len(initial_joint_position)
)
amplitude = math.radians(10.0)
# create a new goal starting with current position
goal = JointState()
goal.position[:] = initial_joint_position
# first motion
goal.position[0] = initial_joint_position[0] + amplitude
goal.position[1] = initial_joint_position[1] - amplitude
self.set_position_goal_joint_publish_and_wait(goal)
# second motion
goal.position[0] = initial_joint_position[0] - amplitude
goal.position[1] = initial_joint_position[1] + amplitude
self.set_position_goal_joint_publish_and_wait(goal)
# back to initial position
goal.position[:] = initial_joint_position
self.set_position_goal_joint_publish_and_wait(goal)
rospy.loginfo(rospy.get_caller_id() + " <- joint goal complete")
def callback(self, data):
if (data.data != "BADINPUT"):
rospy.loginfo(rospy.get_caller_id()+": I heard %s, I will now repeat it.", data.data)
self.parrotpub.publish(data.data)
self.lastsentence = data.data
else:
rospy.loginfo(rospy.get_caller_id()+": I heard %s, I will not repeat it. Please try speaking more clearly.", data.data)
def cartesian_direct(self):
print(rospy.get_caller_id(), ' -> starting cartesian direct')
self.prepare_cartesian()
# create a new goal starting with current position
initial_cartesian_position = PyKDL.Frame()
initial_cartesian_position.p = self.arm.get_desired_position().p
initial_cartesian_position.M = self.arm.get_desired_position().M
goal = PyKDL.Frame()
goal.p = self.arm.get_desired_position().p
goal.M = self.arm.get_desired_position().M
# motion parameters
amplitude = 0.05 # 5 cm
duration = 5 # 5 seconds
rate = 200 # aiming for 200 Hz
samples = duration * rate
for i in range(samples):
goal.p[0] = initial_cartesian_position.p[0] + amplitude * math.sin(i * math.radians(360.0) / samples)
goal.p[1] = initial_cartesian_position.p[1] + amplitude * math.sin(i * math.radians(360.0) / samples)
self.arm.move(goal.p, interpolate=False)
# check error on kinematics, compare to desired on arm.
# to test tracking error we would compare to
# current_position
errorX = goal.p[0] - self.arm.get_desired_position().p[0]
errorY = goal.p[1] - self.arm.get_desired_position().p[1]
errorZ = goal.p[2] - self.arm.get_desired_position().p[2]
error = math.sqrt(errorX * errorX + errorY * errorY + errorZ * errorZ)
if error > 0.002: # 2 mm
print('Inverse kinematic error in position [', i, ']: ', error)
rospy.sleep(1.0 / rate)
print(rospy.get_caller_id(), ' <- cartesian direct complete')
def move_cartesian_frame_linear_interpolation(self, abs_frame, speed, cubInterpolate = False):
""" Linear interpolation of frame using SLERP.
abs_frame as a list of [X Y Z Roll Pitch Yaw]
If cubInterpolate = True -> Speed should be (list) [vLow vHigh] -> Implements cubic slerp interpolation
If cubInterpolate = False -> Speed should be (float) speed -> Implements linear slerp interpolation
:param abs_frame: the absolute `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting absolute move cartesian SLERP')
if (self.__check_input_type(abs_frame, [list,float])):
# Checks for abs_frame as list type
if (cubInterpolate):
if (self.__check_input_type(speed, [list,float])):
if (type(speed) == list and len(speed) == 2):
pass
else:
print "Error in ", inspect.stack()[1][3], " speed should be a list of [vlow vhigh]"
return False
else:
return False
else:
if (not self.__check_input_type(speed, [int,float])):
return False
self.__move_cartesian_SLERP(abs_frame, speed)
rospy.loginfo(rospy.get_caller_id() + ' -> completing absolute move cartesian SLERP')
def head(goal_pos,time_limit):
global start_pos;
motorH1_response = motor_data_client(motorid_H[0]);
motorH2_response = motor_data_client(motorid_H[1]);
start_pos = [motorH1_response.current_pos,motorH2_response.current_pos];
curr_pos = start_pos;
#handlers for motor publishers
H1 = rospy.Publisher('/H1_controller/command', Float64, queue_size=10);
H2 = rospy.Publisher('/H2_controller/command', Float64, queue_size=10);
#initialize node for the specific subpart
#rospy.init_node('Right_arm_node', anonymous=True);
rate = rospy.Rate(update_rate) # 50hz update rate
time.sleep(0.2); # make the system sleep a while
time_count = 0 ;
time_limit = time_limit * update_rate;
while (rospy.is_shutdown() == 0 and time_count <= time_limit ):
global curr_pos;
curr_pos = calculate_trajectory(time_count,start_pos,goal_pos,time_limit);
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to head motor 1" %curr_pos[0] );
H1.publish(curr_pos[0] );
rospy.loginfo(rospy.get_caller_id() + " Publishing %s to head motor 2" %curr_pos[1] );
H2.publish(curr_pos[1] );
time_count = time_count + 1;
time.sleep(0.02);
def calc_vel():
rospy.init_node('calc_vel', anonymous=True)
rospy.loginfo(rospy.get_caller_id() + " started.")
# velocity request and velocity sensor
cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist, cb_vel_in)
odom_sub = rospy.Subscriber("odom", Twist, cb_current_vel)
# effort subs
lin_effort = rospy.Subscriber("lin_effort", controller_msg, cb_lin_effort)
ang_effort = rospy.Subscriber("ang_effort", controller_msg, cb_ang_effort)
# state msgs to pid controller nodes
global lin_state_pub, ang_state_pub, vel_out_pub
lin_state_pub = rospy.Publisher('lin_state', lin_msg, queue_size=10)
ang_state_pub = rospy.Publisher('ang_state', ang_msg, queue_size=10)
# to Arduino
vel_out_pub = rospy.Publisher('vel_out', Twist, queue_size=10)
reset_values()
start_pid_from_cli()
global delta_t, loop_rate
loop_rate = rospy.Rate(1/delta_t)
while not rospy.is_shutdown():
rospy.loginfo(rospy.get_caller_id() + \
"\nlin_x: %f setpoint_x: %f\n ang_z: %f setpoint_z: %f\n",
lin_state.x, lin_state.setpoint,
ang_state.x, ang_state.setpoint, )
simulate_update()
loop_rate.sleep()
def move_cartesian_frame_linear_interpolation(self, abs_frame, speed, cubInterpolate = False):
""" Linear interpolation of frame using SLERP.
abs_frame as a list of [X Y Z Roll Pitch Yaw]
If cubInterpolate = True -> Speed should be (list) [vLow vHigh] -> Implements cubic slerp interpolation
If cubInterpolate = False -> Speed should be (float) speed -> Implements linear slerp interpolation
"""
# convert tfx pose to list
a = abs_frame
abs_frame = [float(a.position.x), float(a.position.y), float(a.position.z), a.rotation.tb_angles.roll_rad, a.rotation.tb_angles.pitch_rad, a.rotation.tb_angles.yaw_rad]
rospy.loginfo(rospy.get_caller_id() + ' -> starting absolute move cartesian SLERP')
if (self.__check_input_type(abs_frame, [list,float])):
# Checks for abs_frame as list type
if (cubInterpolate):
if (self.__check_input_type(speed, [list,float])):
if (type(speed) == list and len(speed) == 2):
pass
else:
print "Error in ", inspect.stack()[1][3], " speed should be a list of [vlow vhigh]"
return False
else:
return False
else:
if (not self.__check_input_type(speed, [int,float])):
return False
self.__move_cartesian_SLERP(abs_frame, speed)
rospy.loginfo(rospy.get_caller_id() + ' -> completing absolute move cartesian SLERP')
def stop(self, applies_to): if len(applies_to) == 0 or len(self.get_mac_addresses() & applies_to) > 0: if self.nodes_are_up(): rospy.loginfo(rospy.get_caller_id() + ": Stopping node process: " + str(self.launch_process)) os.kill(self.launch_process, signal.SIGINT) self.launch_process = None else: rospy.loginfo(rospy.get_caller_id() + ": Did not find running nodes")
def on_speech_info(msg):
if msg.data == "start_speaking":
is_listening = False
rospy.loginfo(rospy.get_caller_id() + ": stop listening")
elif msg.data == "done_speaking":
is_listening = True
rospy.loginfo(rospy.get_caller_id() + ": resume listening")
else:
rospy.loginfo(rospy.get_caller_id() + ": unrecognised speech_info message %s", msg.data)
def verify(data):
verify.r_max.put(data.input)
maximum = verify.r_max.get()
if data.solution == maximum:
rospy.loginfo(rospy.get_caller_id() + "Maximum is correct: %d", data.solution)
return True
else:
rospy.logerr(rospy.get_caller_id() + "Wrong maximum, should be: %d", maximum)
return False
def respond_to(self, heard_text):
if not self.is_speaking:
rospy.loginfo(rospy.get_caller_id() + ": I heard: %s", heard_text)
self.update_count_of_faces_in_view()
utterance = self.bot.respond(heard_text, self.session_id).strip()
if utterance != "":
self.pub.publish(utterance)
rospy.loginfo(rospy.get_caller_id() + ": I said: %s", utterance)
else:
rospy.loginfo(rospy.get_caller_id() + ": still speaking, can't respond to: %s", heard_text)
def callback(data,p):
rospy.loginfo(rospy.get_caller_id() + 'I heard x %s', data.x)
rospy.loginfo(rospy.get_caller_id() + 'I heard y %s', data.y)
u1=((data.x)/1.6)+1500
u2=((data.y)/1.2)+1496
v1=u1
v2=u2
print('erreurPan= ',int(u1))
print('erreurTilt= ',int(u2))
print('')
controlPos(int(u1), int(u2),p)
def __move_joint(self, abs_joint, interpolate = True):
"""Absolute move by vector in joint plane.
:param abs_joint: the absolute position of the joints in terms of a list
:param interpolate: if false the trajectory generator will be used; if true you can bypass the trajectory generator"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting absolute move joint vector')
if(self.__check_input_type(abs_joint, [list,float])):
if (interpolate):
self.__move_joint_goal(abs_joint)
else:
self.__move_joint_direct(abs_joint)
rospy.loginfo(rospy.get_caller_id() + ' -> completing absolute move joint vector')
def on_speech_info(self, msg): if msg.data == "start_speaking": self.listener.is_listening = False rospy.loginfo(rospy.get_caller_id() + ": start speaking") elif msg.data == "done_speaking": self.listener.is_listening = self.is_listening_enabled # remove any segment that might have been stored between the time of the last # segment processed and the start of speaking. self.listener.pop_last_segment() rospy.loginfo(rospy.get_caller_id() + ": done speaking") else: rospy.loginfo(rospy.get_caller_id() + ": unrecognised speech_info message %s", msg.data)
def delta_move_cartesian_frame(self, delta_frame, interpolate=True):
"""Incremental move by Frame in cartesian plane.
:param delta_frame: the incremental `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_ based upon the current position
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting delta move cartesian frame')
# is this a legal frame input
if (self.__check_input_type(delta_frame, [Frame])):
# add the incremental move to the current position, to get the ending frame
end_frame = delta_frame * self.__position_cartesian_desired
# move accordingly
self.move_cartesian_frame(end_frame, interpolate)
rospy.loginfo(rospy.get_caller_id() + ' -> completing delta move cartesian frame')
def move_cartesian_frame(self, abs_frame, interpolate=True):
"""Absolute move by Frame in cartesian plane.
:param abs_frame: the absolute `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting absolute move cartesian frame')
if (self.__check_input_type(abs_frame, [Frame])):
# move based on value of interpolate
if (interpolate):
self.__move_cartesian_goal(abs_frame)
else:
self.__move_cartesian_direct(abs_frame)
rospy.loginfo(rospy.get_caller_id() + ' -> completing absolute move cartesian frame')
def delta_move_cartesian_rotation(self, delta_rotation, interpolate=True):
"""Incremental rotation in cartesian plane.
:param delta_rotation: the incremental `PyKDL.Rotation <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_ based upon the current position
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting delta move cartesian rotation')
# is this a legal rotation input
if(self.__check_input_type(delta_rotation, [Rotation])):
# convert into a Frame
delta_vector = Vector(0.0, 0.0, 0.0)
delta_frame = Frame(delta_rotation, delta_vector)
# move accordingly
self.delta_move_cartesian_frame(delta_frame, interpolate)
rospy.loginfo(rospy.get_caller_id() + ' -> completing delta move cartesian rotation')
def move_cartesian_rotation(self, abs_rotation, interpolate=True):
"""Absolute rotation in cartesian plane.
:param abs_rotation: the absolute `PyKDL.Rotation <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_
:param interpolate: see :ref:`interpolate <interpolate>`"""
rospy.loginfo(rospy.get_caller_id() + ' -> starting absolute move cartesian rotation')
# is this a legal rotation input
if(self.__check_input_type(abs_rotation, [Rotation])):
# convert into a Frame
abs_vector = self.__position_cartesian_desired.p
abs_frame = Frame(abs_rotation, abs_vector)
# move accordingly
self.move_cartesian_frame(abs_frame, interpolate)
rospy.loginfo(rospy.get_caller_id() + ' -> completing absolute move cartesian rotation')
def text_message_record(data):
print data.data
if data.data == startValue:
rospy.loginfo(rospy.get_caller_id()+"Start Recording!")
global procMessage
procMessage = subprocess.Popen(["rosbag", "record", "-a","-O", startValue], preexec_fn=os.setsid)
elif data.data == endValue:
rospy.loginfo(rospy.get_caller_id()+"STOPPING RECORDING")
if 'procMessage' in globals():
rospy.loginfo(rospy.get_caller_id()+"Killing Rosbag")
terminate_ros_node("/record")
else:
rospy.loginfo(rospy.get_caller_id()+"NO BUENO")
def callback(data):
rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data)
pub = rospy.Publisher('return', String, queue_size=10)
rate = rospy.Rate(1)
rate.sleep()
pub.publish(data.data)
def callback(data):
'''listener Callback Function'''
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "\nReceived cmd: %s\n %s",
data.linear, data.angular)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.latitude)
camera.capture('%s.jpg'%(data.header.seq))
def callback(data, *args):
rospy.loginfo(rospy.get_caller_id() + " Subscribe: %s", data.data)
ws = args[0]
ws.send(data.data)
def odom_callback(data):
rx = data.pose.pose.position.x
ry = data.pose.pose.position.y
rospy.loginfo(rospy.get_caller_id() + ": %.2f %.2f", rx, ry)
def callback(message):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", message.data)
def callback(data):
global command
print(data.data)
rospy.loginfo(rospy.get_caller_id() + ' received ' + data.data)
command = data.data
def callback(cmd_vel):
rospy.loginfo(rospy.get_caller_id() + "I heard %f", (cmd_vel.linear.x))
#SpeedAccelDistanceM1(self,address,accel,speed,distance,buffer)
rc.SpeedAccelDistanceM1(address, 0, int(cmd_vel.linear.x * 3000), 100000,
1)
def __str__(self):
return rospy.get_caller_id()
def callback(data):
rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data)
file = open("test3.txt", "a")
elapsed = data.data
file.write('%r\n' % (elapsed))
file.close()
def callback(data): soundhandle.say(data.data, voice, volume) rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def on_cmd_str(self, msg):
rospy.loginfo(rospy.get_caller_id() + ' cmd_str=%s', msg.data)
def callback3(data):
global grabbed
rospy.loginfo(rospy.get_caller_id() + " data3 = %s", data.data)
grabbed = data.data
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.Sentences) # not sure if i need this
def callback(self, data):
contents = data.content
options = data.options
rospy.loginfo(rospy.get_caller_id() +
"I heard '%s' with '%s' options" % (contents, options))
def jointStateImpedance_callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def renderAudio(data):
audioID = data.data
if(audioID == "EPIC"):
rospy.loginfo(rospy.get_caller_id() + 'Starting playback of %s', data.data)
os.system("aplay ~/Epicsax.wav")
def reset():
rospy.wait_for_service('/model_client/rl_service')
rospy.loginfo(rospy.get_caller_id() + 'begin service')
reset_step = rospy.ServiceProxy('/model_client/rl_service', RLSrv)
resp1 = reset_step(1, [], [])
return resp1.next_state, resp1.reward, resp1.done
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "Physical manager catchs: %s",
data.data)
def callback(msgmag):
rospy.loginfo(rospy.get_caller_id() + "%f", msgmag.mag)
def callbackInt(data):
rospy.loginfo(rospy.get_caller_id() + "I heard Int %s", data)
#print(data.split(':')[1])
SetAngle(data.data)
from tf.transformations import quaternion_matrix
from geometry_msgs.msg import TransformStamped, Transform
# Get camera registration
scriptDirectory = os.path.dirname(os.path.abspath(__file__))
filePath = os.path.join(scriptDirectory, '..', '..', 'defaults',
'registration_params.yaml')
with open(filePath, 'r') as f:
data = yaml.load(f)
camTransform = np.array(data['transform'])
# create node
if not rospy.get_node_uri():
rospy.init_node('tf_synch_test', anonymous=True, log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
frameRate = 15
slop = 1.0 / frameRate
cams = StereoCameras("/stereo/left/image_rect",
"/stereo/right/image_rect",
"/stereo/left/camera_info",
"/stereo/right/camera_info",
slop=slop)
camModel = StereoCameraModel()
topicLeft = rospy.resolve_name("/stereo/left/camera_info")
msgL = rospy.wait_for_message(topicLeft, CameraInfo, 3)
topicRight = rospy.resolve_name("/stereo/right/camera_info")
msgR = rospy.wait_for_message(topicRight, CameraInfo, 3)
camModel.fromCameraInfo(msgL, msgR)
def step(state, action):
rospy.wait_for_service('/model_client/rl_service')
rospy.loginfo(rospy.get_caller_id() + 'begin service')
take_step = rospy.ServiceProxy('/model_client/rl_service', RLSrv)
resp1 = take_step(0, state.tolist(), action.tolist())
return resp1.next_state, resp1.reward, resp1.done
route['motors'] = {}
for i in range(len(traj.points)):
route['times'][i] = traj.points[i].time_from_start.secs + traj.points[
i].time_from_start.nsecs / 1000000000.0
for m, motor in enumerate(traj.joint_names):
route['motors'][motor] = len(traj.points) * [0]
for i in range(len(traj.points)):
route['motors'][motor][i] = traj.points[i].positions[m]
return interpolate(route, freq)
### MAIN
node_name = 'poppy_control_moveit'
rospy.loginfo(rospy.get_caller_id() + ' Node created')
rospy.init_node(node_name, anonymous=False)
rospy.loginfo(rospy.get_caller_id() + ' Connecting with moveit...')
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
groups_names = robot.get_group_names()
# groups_names = ['r_arm_2']
scene = moveit_commander.PlanningSceneInterface()
groups = {}
for name in groups_names:
groups[name] = moveit_commander.MoveGroupCommander(name)
groups[name].set_planner_id(name)
groups[name].set_planning_time(5)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', \
moveit_msgs.msg.DisplayTrajectory, \
queue_size=20)
def callback(data):
print rospy.get_caller_id(), "I heard %s, %s, %s" % (
data.str1.data, data.int1.data, data.val.val)
start_publishing()
print "re-publishing"
_pub.publish(data)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + " got some data: %s", data.data)
self.lastData = data.data
self.state = True
def callback(data):
print rospy.get_caller_id(), "I heard %s"%data.data
start_publishing()
print "publishing", data.data
_pub.publish(String(data.data))
def web_interface_callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s ", data.data)
self.human_input = data.data
