def execute(self, userdata):
sss.move("gripper", "open", blocking=False)
# sss.move("arm", "zeroposition")
for object in userdata.object_list:
# ToDo: need to be adjusted to correct stuff
if object.pose.pose.position.z <= 0.0 or object.pose.pose.position.z >= 0.10:
continue
sss.move("arm", "zeroposition", mode="planned")
#object.pose.pose.position.z = object.pose.pose.position.z + 0.02
object.pose.pose.position.x = object.pose.pose.position.x + 0.01
object.pose.pose.position.y = object.pose.pose.position.y - 0.005
handle_arm = sss.move("arm", [object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"], mode="planned")
if handle_arm.get_state() == 3:
sss.move("gripper", "close", blocking=False)
rospy.sleep(3.0)
sss.move("arm", "zeroposition", mode="planned")
return 'succeeded'
else:
rospy.logerr('could not find IK for current object')
return 'failed'
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def servo_to_joints_cb(self,req):
if self.driver_status == 'SERVO':
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
stamp = self.acquire()
# inverse kinematics
traj = self.planner.getJointMove(req.target.position,
self.q0,
self.base_steps,
self.steps_per_meter,
self.steps_per_radians,
time_multiplier = (1./velocity),
percent_acc = acceleration,
use_joint_move = True,
table_frame = self.table_pose)
# Send command
if len(traj.points) > 0:
if stamp is not None:
rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False)
self.release()
else:
res = 'FAILURE -- could not preempt current arm control.'
return res
else:
rospy.logerr('SIMPLE DRIVER -- IK failed')
return 'FAILURE -- no trajectory points'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE -- not in servo mode'
def solve(self, ptarget, target_rot): #ptarget should be a Point(); target_rot 'FRONT' or 'DOWN'
# self.solution_found = False
ikreq = SolvePositionIKRequest() #service request object
hdr = Header(
stamp=rospy.Time.now(), frame_id='base')
if target_rot == 'FRONT':
pose = PoseStamped(
header=hdr,
pose=Pose(
position=ptarget,
orientation=self.qtarget[0]
),
)
elif target_rot == 'DOWN':
pose = PoseStamped(
header=hdr,
pose=Pose(
position=ptarget,
orientation=self.qtarget[1]
),
)
ikreq.pose_stamp.append(pose)
try:
resp = self.iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
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 handle_start_srv(req):
try:
p=Popen(['roslaunch', 'jsk_pepper_startup', 'play_audio_stream.launch'])
return EmptyResponse()
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return res
def smart_move_cb(self,req):
'''
Find any valid object that meets the requirements.
Find a cartesian path or possibly longer path through joint space.
'''
if not self.driver_status == 'SERVO':
msg = 'FAILURE -- not in servo mode'
rospy.logerr('DRIVER -- Not in servo mode!')
return msg
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
possible_goals = self.query(req)
stamp = self.acquire()
if len(possible_goals) == 0:
return 'FAILURE -- no valid poses found. see costar_arm.py'
for (dist,T,obj,name) in possible_goals:
rospy.logwarn("Trying to move to frame at distance %f"%(dist))
# plan to T
if not self.valid_verify(stamp):
msg = 'FAILURE - Stopping smart move action because robot has been preempted by another process. see costar_arm.py'
rospy.logwarn(msg)
return msg
(code,res) = self.planner.getPlan(T,self.q0,obj=obj)
msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity)
if msg[0:7] == 'SUCCESS':
break
return msg
def main():
rospy.init_node("find_fiducial_pose_test")
# Construct action ac
rospy.loginfo("Starting action client...")
action_client = actionlib.SimpleActionClient("find_fiducial_pose", FindFiducialAction)
action_client.wait_for_server()
rospy.loginfo("Action client connected to action server.")
# Call the action
rospy.loginfo("Calling the action server...")
action_goal = FindFiducialGoal()
action_goal.camera_name = "/camera/rgb"
action_goal.pattern_width = 7
action_goal.pattern_height = 6
action_goal.pattern_size = 0.027
action_goal.pattern_type = 1 # CHESSBOARD
if (
action_client.send_goal_and_wait(action_goal, rospy.Duration(50.0), rospy.Duration(50.0))
== GoalStatus.SUCCEEDED
):
rospy.loginfo("Call to action server succeeded")
else:
rospy.logerr("Call to action server failed")
def serWrite(myStr):
try:
for i in range(len(myStr)):
ser.write(myStr[i])
rospy.sleep(0.0001) # Sometimes, a delay seems to help. Maybe?
except:
rospy.logerr("[GS] Unable to send data. Check connection.")
def execute(self, userdata):
rospy.loginfo(OKGREEN+"i'm looking what tags are"+ENDC)
rospy.loginfo(OKGREEN+str(userdata.asr_userSaid)+ENDC)
#rospy.loginfo(OKGREEN+"TAGS: "+str(self.tags)+ENDC)
#rospy.loginfo(OKGREEN+str(userdata.asr_userSaid_tags)+ENDC)
#userdata.object=userdata.asr_userSaid # it means that in this place it have a coke
if userdata.asr_userSaid=="finish" :
rospy.logwarn("-------------------------------------i'm have a finish order")
return 'finish'
listTags = userdata.asr_userSaid_tags
#Process tags
userdata.object_array = []
locationValue = [tag for tag in listTags if tag.key == 'direction']
objectValue = [tag for tag in listTags if tag.key == 'class']
if objectValue and locationValue:
userdata.objectOrientation = locationValue[0].value
userdata.objectName = objectValue[0].value
return 'new_position'
else:
rospy.logerr("Object or Location not set")
return 'aborted'
def run_spawn_hook(hook):
try:
hook()
except:
rospy.logerr("Caught an Exception while running a spawn hook!")
# Log the traceback.
rospy.logerr(sys.exc_info()[2])
def verify_boards(self, check):
if self.has_finished:
return False
try:
rospy.wait_for_service('mcb_conf_results', 15)
except:
msg = "MCB conf results service not available. Unable to configure MCB's"
rospy.logerr(msg)
self.finished(False, msg)
return False
if not self.count_boards():
return False
if not self.check_link():
return False
if not self.get_serials():
return False
if check:
return self.check_boards()
return True
def main():
try:
opts, args = getopt.getopt(sys.argv[1:], 'hc:',
['help', 'calibrate=',])
except getopt.GetoptError as err:
print str(err)
usage()
sys.exit(2)
arm = None
for o, a in opts:
if o in ('-h', '--help'):
usage()
sys.exit(0)
elif o in ('-c', '--calibrate'):
arm = a
if not arm:
usage()
rospy.logerr("No arm specified")
sys.exit(1)
rospy.init_node('calibrate_arm_sdk', anonymous=True)
rs = baxter_interface.RobotEnable()
rs.enable()
cat = CalibrateArm(arm)
rospy.loginfo("Running calibrate on %s arm" % (arm,))
error = None
try:
cat.run()
except Exception, e:
error = str(e)
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):
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 __init__(self, port=None, baud=57600, timeout=5.0):
""" Initialize node, connect to bus, attempt to negotiate topics. """
self.mutex = thread.allocate_lock()
self.lastsync = rospy.Time(0)
self.lastsync_lost = rospy.Time(0)
self.timeout = timeout
self.synced = False
self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
if port== None:
# no port specified, listen for any new port?
pass
elif hasattr(port, 'read'):
#assume its a filelike object
self.port=port
else:
# open a specific port
try:
self.port = Serial(port, baud, timeout=self.timeout*0.5)
except SerialException as e:
rospy.logerr("Error opening serial: %s", e)
rospy.signal_shutdown("Error opening serial: %s" % e)
raise SystemExit
self.port.timeout = 0.01 # Edit the port timeout
time.sleep(0.1) # Wait for ready (patch for Uno)
# hydro introduces protocol ver2 which must match node_handle.h
# The protocol version is sent as the 2nd sync byte emitted by each end
self.protocol_ver1 = '\xff'
self.protocol_ver2 = '\xfe'
self.protocol_ver = self.protocol_ver2
self.publishers = dict() # id:Publishers
self.subscribers = dict() # topic:Subscriber
self.services = dict() # topic:Service
self.buffer_out = -1
self.buffer_in = -1
self.callbacks = dict()
# endpoints for creating new pubs/subs
self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
# service client/servers have 2 creation endpoints (a publisher and a subscriber)
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
# custom endpoints
self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest
rospy.sleep(2.0) # TODO
self.requestTopics()
self.lastsync = rospy.Time.now()
def free_capability(self, capability_interface, timeout=None):
"""Free's a previously used capability.
Calls the ``~free_capability`` service, which effectively decrements
the internal reference count for that capability in the remote
capability server.
If that results in a reference count of zero,
then the capability server will shutdown that capability automatically.
:param capability_interface: Name of the capability interface to free up
:type capability_interface: str
:param timeout: time to wait on service to be available (optional)
:type timeout: float
:returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
:rtype: :py:obj:`bool`
:raises: CapabilityNotInUseException if the capability has not been previously used
:raises: CapabilityNotRunningException if the capability has already been stopped
which can be caused by a capability it depends on stopping
:raises: ServiceNotAvailableException if the required service is not available
after the timeout has expired
"""
if capability_interface not in self._used_capabilities:
rospy.logerr("Cannot free capability interface '{0}', because it was not first used."
.format(capability_interface))
CapabilityNotInUseException(capability_interface)
if self.__wait_for_service(self.__free_capability, timeout) is False:
raise ServiceNotAvailableException(self.__free_capability.resolved_name)
try:
self.__free_capability.call(capability_interface, self._bond_id)
except rospy.ServiceException as exc:
exc_str = "{0}".format(exc)
if 'Cannot free Capability' in exc_str and 'because it is not running' in exc_str:
raise CapabilityNotRunningException(capability_interface)
return True
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time(0), timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
except:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], tf.transformations.quaternion_from_euler(0,math.pi/2,0))
else:
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], [0,0,0])
actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
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 getPointDist(self, pt):
assert(self.face is not None)
# fist, get my position
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = 0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
try:
self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(self._robot_frame, p)
except:
rospy.logerr("TF error!")
return None
return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))
def getPoseStamped(self, group, c):
assert(len(c) == 6)
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = c[0]
p.pose.position.y = c[1]
p.pose.position.z = c[2]
quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5])
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
try:
self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(group.get_pose_reference_frame(), p)
except:
rospy.logerr("TF error!")
return None
return p
def scan_callback(self, msg):
angle = msg.angle_min
d_angle = msg.angle_increment
sum_x = 0
sum_y = 0
sum_xx = 0
sum_xy = 0
num = 0
for r in msg.ranges:
if angle > self.min_angle and angle < self.max_angle and r < msg.range_max:
x = math.sin(angle) * r
y = math.cos(angle) * r
sum_x += x
sum_y += y
sum_xx += x*x
sum_xy += x*y
num += 1
angle += d_angle
if num > 0:
denominator = num*sum_xx-sum_x*sum_x
if denominator != 0:
angle=math.atan2((-sum_x*sum_y+num*sum_xy)/(denominator), 1)
#print("Scan Angle: %s"%str(angle))
relay = ScanAngle()
relay.header = msg.header
relay.scan_angle = angle
with self.lock:
if self._laser_scan_angle_publisher:
self._laser_scan_angle_publisher.publish(relay)
else:
rospy.logerr("Please point me at a wall.")
def main():
rospy.init_node("sonarFilter", anonymous=False)
filterChain = SonarFilter()
try:
rospy.spin()
except rospy.ROSInterruptException:
rospy.logerr("sonarFilter interrupted")
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [x
for x in self.__capability_instances.values()
if x.state == 'waiting']
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency_name in instance.depends_on:
if dependency_name not in self.__capability_instances: # pragma: no cover
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on "
.format(instance.name) + "'{0}', ".format(dependency_name) +
"which is not in the list of capability instances.")
return
dependency = self.__capability_instances[dependency_name]
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path
)
# Remove any terminated capabilities
self.__remove_terminated_capabilities()
def rosImgToCVMono(self, rosImg):
try:
cvImg = self.bridge.imgmsg_to_cv2(rosImg, desired_encoding="mono8")
cvImg = cv2.cvtColor(cvImg, cv2.COLOR_GRAY2BGR)
except CvBridgeError as e:
rospy.logerr(e)
return cvImg
def parse_usbllong(self, tokens):
"""
method which will generate a ros message from USBL data
:param tokens: list of message fields
"""
if len(tokens) != 17:
rospy.logerr("%s: USBLLONG message fields count doesn't match" % self.name)
return
usblmsg = AcousticModemUSBLLONG()
usblmsg.header.stamp = rospy.Time.now()
usblmsg.measurement_time = float(tokens[2])
usblmsg.remote_address = int(tokens[3])
usblmsg.X = float(tokens[4])
usblmsg.Y = float(tokens[5])
usblmsg.Z = float(tokens[6])
usblmsg.E = float(tokens[7])
usblmsg.N = float(tokens[8])
usblmsg.U = float(tokens[9])
usblmsg.roll = float(tokens[10]) # RPY of local device
usblmsg.pitch = float(tokens[11])
usblmsg.yaw = float(tokens[12])
usblmsg.propagation_time = float(tokens[13])
usblmsg.accuracy = float(tokens[-1])
rospy.loginfo('%s: Received USBLLONG data' %(self.name))
self.received_cnt += 1
self.pub_usbllong.publish(usblmsg)
def parse_usblangles(self, tokens):
"""
method which will generate a ros message from USBL data
:param tokens: list of message fields
"""
if len(tokens) != 14:
rospy.logerr("%s: USBLANGLES message fields count doesn't match" % self.name)
return
usblmsg = AcousticModemUSBLANGLES()
usblmsg.header.stamp = rospy.Time.now()
usblmsg.measurement_time = float(tokens[2])
usblmsg.remote_address = int(tokens[3])
usblmsg.lbearing = float(tokens[4])
usblmsg.lelevation = float(tokens[5])
usblmsg.bearing = float(tokens[6])
usblmsg.elevation = float(tokens[7])
usblmsg.roll = float(tokens[8]) # RPY of local device
usblmsg.pitch = float(tokens[9])
usblmsg.yaw = float(tokens[10])
usblmsg.accuracy = float(tokens[13])
rospy.loginfo('%s: Received USBLANGLES data' %(self.name))
self.received_cnt += 1
self.pub_usblangles.publish(usblmsg)
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 get_param_duration(param):
"""Calls rospy.get_param and logs errors.
Logs if the param does not exist or is not parsable to rospy.Durotation.
And calls rospy.signal_shutdown if the value is invalid/not existing.
:return: The Param param from the parameter server.
:rtype: rospy.Duration
"""
# dummy value
value = rospy.Duration(1)
try:
# only a default value in case the param gets fuzzed.
value = rospy.Duration(get_param_num(param))
except ValueError:
err_msg = (
"Param %s has the invalid value '%s'."
% (param, rospy.get_param(param)))
rospy.logerr(err_msg)
rospy.signal_shutdown(err_msg)
value = rospy.Duration(1)
return value
def stretchingAction(self):
self.say("I'm bit tired. Let's do some stretching.")
self._torso_action_cl.wait_for_server()
goal = pr2_controllers_msgs.msg.SingleJointPositionGoal()
goal.position = 0.195
goal.max_velocity = 0.5
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (up)")
# TODO move arms
self.say("I feel much better now!")
goal.position = 0.0
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (down)")
def change_motor_config_file(hw_version, can_id_list, dxl_id_list):
rospack = rospkg.RosPack()
folder_path = rospack.get_path('niryo_one_bringup')
file_path = folder_path + '/config/v' + str(int(hw_version)) + '/niryo_one_motors.yaml'
lines = []
can_bus_enabled = len(can_id_list) > 0
dxl_bus_enabled = len(dxl_id_list) > 0
rospy.loginfo("Change motor config file (for debug purposes): " + str(file_path))
rospy.loginfo("Hw version: " + str(hw_version))
rospy.loginfo("CAN ID list: " + str(can_id_list))
rospy.loginfo("DXL ID list: " + str(dxl_id_list))
if hw_version != 1 and hw_version != 2:
return CHANGE_MOTOR_CONFIG_WRONG_VERSION
# Open file, get text and split lines
try:
with open(file_path, 'r') as f:
lines = f.read().splitlines()
except EnvironmentError as e:
rospy.logerr(e)
return CHANGE_MOTOR_CONFIG_READ_FAIL
text_begin_lines = []
text_can_lines = []
text_dxl_lines = []
text_end_lines = []
# Get beginning of file (not modified)
for line in lines:
if not line.startswith('#'):
break
text_begin_lines.append(line)
# Get end of file (not modified)
for i, line in enumerate(lines):
if line.startswith('dxl_authorized_motors'):
text_end_lines = lines[i:]
break
# Fill CAN motor IDs depending on given ID array
# If CAN bus is already disabled, then don't disable motors here
if hw_version == 1:
text_can_lines.append('can_required_motors: # Axis 1-4 of Niryo One (stepper 1 -> id 1, stepper 2 -> id 2, ...)')
else:
text_can_lines.append('can_required_motors: # Axis 1-3 of Niryo One (stepper 1 -> id 1, stepper 2 -> id 2, ...)')
text_can_lines.append(' # Edit only for debug purposes (ex : you want to test some motors separately and detached from the robot)')
text_can_lines.append(' # --> Commented ids will make associated motor disable (and thus not trigger an error if not connected)')
text_can_lines.append(' # Before editing, please be sure that you know what you\'re doing')
if 1 in can_id_list or not can_bus_enabled:
text_can_lines.append(' - 1 # Axis 1 enabled if not commented')
else:
text_can_lines.append(' #- 1 # Axis 1 enabled if not commented')
if 2 in can_id_list or not can_bus_enabled:
text_can_lines.append(' - 2 # Axis 2 enabled if not commented')
else:
text_can_lines.append(' #- 2 # Axis 2 enabled if not commented')
if 3 in can_id_list or not can_bus_enabled:
text_can_lines.append(' - 3 # Axis 3 enabled if not commented')
else:
text_can_lines.append(' #- 3 # Axis 3 enabled if not commented')
if hw_version == 1:
if 4 in can_id_list or not can_bus_enabled:
text_can_lines.append(' - 4 # Axis 4 enabled if not commented')
else:
text_can_lines.append(' #- 4 # Axis 4 enabled if not commented')
# Fill DXL motor IDs depending on given ID array
# If DXL bus is already disabled, then don't disable motors here
if hw_version == 1:
text_dxl_lines.append('dxl_required_motors: # axis 5 and 6 of Niryo One.')
else:
text_dxl_lines.append('dxl_required_motors: # axis 4, 5 and 6 of Niryo One.')
text_dxl_lines.append(' # Edit only for debug purposes (ex : you want to test some motors separately and detached from the robot)')
text_dxl_lines.append(' # --> Commented ids will make associated motor disable (and thus not trigger an error if not connected)')
text_dxl_lines.append(' # Before editing, please be sure that you know what you\'re doing')
if hw_version == 1:
text_dxl_lines.append(' # - Note : Axis 5 has 2 motors, but you can use only one motor for this axis when debugging')
if hw_version == 1:
if 4 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 4 # -> id of Axis 5_1')
else:
text_dxl_lines.append(' #- 4 # -> id of Axis 5_1')
if 5 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 5 # -> id of Axis 5_2')
else:
text_dxl_lines.append(' #- 5 # -> id of Axis 5_2')
if 6 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 6 # -> id of Axis 6')
else:
text_dxl_lines.append(' #- 6 # -> id of Axis 6')
else:
if 2 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 2 # -> id of Axis 4')
else:
text_dxl_lines.append(' #- 2 # -> id of Axis 4')
if 3 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 3 # -> id of Axis 5')
else:
text_dxl_lines.append(' #- 3 # -> id of Axis 5')
if 6 in dxl_id_list or not dxl_bus_enabled:
text_dxl_lines.append(' - 6 # -> id of Axis 6')
else:
text_dxl_lines.append(' #- 6 # -> id of Axis 6')
new_text = '\n'.join(text_begin_lines + [''] + text_can_lines + [''] + text_dxl_lines + [''] + text_end_lines + [''])
# Rewrite file with new text
try:
with open(file_path, 'w') as f:
f.write(new_text)
except EnvironmentError as e:
rospy.logerr(e)
return CHANGE_MOTOR_CONFIG_WRITE_FAIL
return CHANGE_MOTOR_CONFIG_SUCCESS
rospy.Subscriber("/clicked_point", PointStamped, self.insert_marker_callback)
rospy.Subscriber("/clicked_pose", PoseStamped, self.insert_terminal_marker_callback)
rospy.on_shutdown(self.clear_all_markers)
# rospy.logwarn("The waypoint server is waiting for RViz to run and to be subscribed to {0}.".format(rospy.resolve_name("~edges")))
# while self.edge_line_publisher.get_num_connections() == 0:
# rospy.sleep(1.0)
self.clear_all_markers()
self.waypoint_file = rospy.get_param("~waypoint_file", "") # yaml file with waypoints to load
if len(self.waypoint_file) != 0:
rospy.loginfo("Waypoint_Server is loading initial waypoint file {0}.".format(self.waypoint_file))
error_message = self.load_waypoints_from_file(self.waypoint_file)
if error_message:
rospy.logerr(error_message)
def get_waypoint_data_service(self, req):
response = GetWaypointDataResponse()
response.waypoint_found = True
<<<<<<< HEAD
def get_valid_locations(self):
valid_locations = {}
for uuid, waypoint_data in self.waypoint_graph.nodes(data=True):
if waypoint_data['waypoint_type'] == WAYPOINT_TERMINATING:
name = self.uuid_name_map[uuid]
pt.pose.orientation.x = q[0]
pt.pose.orientation.y = q[1]
pt.pose.orientation.z = q[2]
pt.pose.orientation.w = q[3]
moveit_test.create_plan_to_target("left_arm", pt)
q = (kdl.Rotation.RPY(1.57,0,-1.57)).GetQuaternion()
pt = geometry_msgs.msg.PoseStamped()
pt.header.frame_id = "world"
pt.header.seq = 0
pt.header.stamp = rospy.Time.now()
pt.pose.position.x = 0.3
pt.pose.position.y = -0.5
pt.pose.position.z = 1.2
pt.pose.orientation.x = q[0]
pt.pose.orientation.y = q[1]
pt.pose.orientation.z = q[2]
pt.pose.orientation.w = q[3]
moveit_test.create_plan_to_target("right_arm", pt)
r1 = moveit_test.execute_plan("left_arm")
r2 = moveit_test.execute_plan("right_arm")
if not r1 : rospy.logerr("moveit_test(left_arm) -- couldn't execute plan")
if not r2 : rospy.logerr("moveit_test(right_arm) -- couldn't execute plan")
moveit_commander.roscpp_shutdown()
except rospy.ROSInterruptException:
pass
if(stop_line_idx != stop_forward_idx):
#likely car is away from stop line still ?
if(self.debug):
rospy.loginfo('traffic light upcoming but there\s no stop line position found')
return -1, TrafficLight.UNKNOWN
# 3. Find the car waypoint closest to the stop line
stop_waypoint_idx = self.get_closest_waypoint(
self.stop_line_positions_poses[stop_line_idx].pose.pose,
self.waypoints.waypoints )
if( stop_waypoint_idx == None ):
rospy.loginfo('Couldnt find waypoint in process_traffic_lights()')
return -1, TrafficLight.UNKNOWN
use_detector_flag = True
if use_detector_flag:
state = self.get_light_state( self.lights[light_idx] )
else:
state = self.lights[light_idx].state #this is only valid within simulator
rospy.loginfo(self.tlclasses_d[ state ] )
return stop_waypoint_idx, state
if __name__ == '__main__':
try:
TLDetector()
except rospy.ROSInterruptException:
rospy.logerr('Could not start traffic node.')
global found_button
# Button pressed position gathered from Gazebo analysis
buttonPos = Vector3(3.35, -0.73, 1.25)
offset = Vector3(center.point.x - buttonPos.x,
center.point.y - buttonPos.y,
center.point.z - buttonPos.z)
zarj_eyes.remove_detection_request(name)
zarj_tf.set_world_transform(offset)
found_button = True
if __name__ == '__main__':
try:
rospy.init_node('team_zarj_qual2')
if not rospy.has_param('/ihmc_ros/robot_name'):
rospy.logerr("Cannot run team_zarj_qual2.py, missing parameters!")
rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")
else:
robot_name = rospy.get_param('/ihmc_ros/robot_name')
right_foot_frame_parameter_name = "/ihmc_ros/{0}/right_foot_frame_name".format(robot_name)
left_foot_frame_parameter_name = "/ihmc_ros/{0}/left_foot_frame_name".format(robot_name)
if rospy.has_param(right_foot_frame_parameter_name) and rospy.has_param(left_foot_frame_parameter_name):
control = "/ihmc_ros/{0}/control/".format(robot_name)
output = "/ihmc_ros/{0}/output/".format(robot_name)
BodyPublisher = rospy.Publisher(control + "whole_body_trajectory", WholeBodyTrajectoryRosMessage, queue_size=10)
footStepListPublisher = rospy.Publisher(control + "footstep_list", FootstepDataListRosMessage, queue_size=10)
handTrajectoryPublisher = rospy.Publisher(control + "hand_trajectory", HandTrajectoryRosMessage, queue_size=10)
def CamRgbImageCallback(self, rgb_image_data):
try:
rgb_image = self.cv_bridge.imgmsg_to_cv2(rgb_image_data, 'passthrough')
except CvBridgeError, e:
rospy.logerr(e)
def CamDepthImageCallback(self, depth_image_data):
try:
self.m_depth_image = self.cv_bridge.imgmsg_to_cv2(depth_image_data, 'passthrough')
except CvBridgeError, e:
rospy.logerr(e)
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
def __init__(self):
self.odom_freq = float(rospy.get_param('~odom_freq',
'50')) # hz of odom pub
self.odom_topic = rospy.get_param('~odom_topic',
'/odom2base_footprint') # topic name
self.baseId = rospy.get_param('~base_id',
'base_footprint') # base link
self.odomId = rospy.get_param('~odom_id', 'odom') # odom link
self.VxCov = float(rospy.get_param(
'~vx_cov', '1.0')) # covariance for Vx measurement
self.VyawCov = float(rospy.get_param(
'~vyaw_cov', '1.0')) # covariance for Vyaw measurement
self.pub_tf = bool(rospy.get_param(
'~pub_tf', True)) # whether publishes TF or not
self.wheelRad = float(0.072 / 2.0) #m
self.wheelSep = float(0.17) #m
self.scale = rospy.get_param('~scale', '1.0')
try:
self.serial = serial.Serial('/dev/due_controller',
115200,
timeout=0.5)
rospy.loginfo("Connect success ...")
try:
print("Flusing first 50 data readings ...")
for x in range(0, 50):
data = self.serial.read()
time.sleep(0.01)
except:
print("Flusing faile ")
sys.exit(0)
except serial.serialutil.SerialException:
rospy.logerr(
"Can not receive data from the port: " #+ self.device_port +
". Did you specify the correct port ?")
self.serial.close
sys.exit(0)
rospy.loginfo("Communication success !")
# rospy.loginfo("Flusing first 50 data readings ...")
# for x in range(0, 50):
# self.serial.readline().strip()
# time.sleep(0.01)
# ROS handler
self.sub = rospy.Subscriber('/car/cmd_vel',
Twist,
self.cmdCB,
queue_size=10)
self.pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10)
self.timer_odom = rospy.Timer(rospy.Duration(0.1), self.timerOdomCB)
self.timer_cmd = rospy.Timer(rospy.Duration(0.1),
self.timerCmdCB) # 10Hz
self.tf_broadcaster = tf.TransformBroadcaster()
# variable
self.trans_x = 0.0 # cmd
self.rotat_z = 0.0
self.WL_send = 0.0
self.WR_send = 0.0
self.current_time = rospy.Time.now()
self.previous_time = rospy.Time.now()
self.pose_x = 0.0 # SI
self.pose_y = 0.0
self.pose_yaw = 0.0
def traffic_cb(self, msg):
self.stopline_wp_idx = msg.data
def obstacle_cb(self, msg):
# TODO: Callback for /obstacle_waypoint message. We will implement it later
pass
def get_waypoint_velocity(self, waypoint):
return waypoint.twist.twist.linear.x
def set_waypoint_velocity(self, waypoints, waypoint, velocity):
waypoints[waypoint].twist.twist.linear.x = velocity
def distance(self, waypoints, wp1, wp2):
dist = 0
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
for i in range(wp1, wp2 + 1):
dist += dl(waypoints[wp1].pose.pose.position,
waypoints[i].pose.pose.position)
wp1 = i
return dist
if __name__ == '__main__':
try:
WaypointUpdater()
except rospy.ROSInterruptException:
rospy.logerr('Could not start waypoint updater node.')
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller",
False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state',
SensorState,
queue_size=5)
# A service to position a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
# A service to read the position of a PWM servo
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# A service to turn set the direction of a digital pin (0 = input, 1 = output)
rospy.Service('~digital_set_direction', DigitalSetDirection,
self.DigitalSetDirectionHandler)
# A service to turn a digital sensor on or off
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
# A service to read the value of a digital sensor
rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
# A service to set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# A service to read the value of an analog sensor
rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout,
self.motors_reversed)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
str(self.baud) + " baud")
# Reserve a thread lock
mutex = _thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
for name, params in sensor_params.items():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
if params['type'] == "Ping":
sensor = Ping(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'],
params['rate'], self.base_frame)
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
try:
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params) +
" published on topic " + rospy.get_name() +
"/sensor/" + name)
except:
rospy.logerr("Sensor type " + str(params['type']) +
" not recognized.")
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(
self.controller, self.base_frame,
self.name + "_base_controller")
# Start polling the sensors and base controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
# Publish all sensor values on a single topic for convenience
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
def place(self,place_goal):
placeresult = PlaceResult()
target_pose_to_execute_ = PoseStamped()
#for location, check path from approach pose to release pose first and then check motion to approach pose
motion_plan_res=GetMotionPlanResponse()
object_to_attach = place_goal.collision_object_name
# get current hand pose
self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0))
try:
(trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("Cannot get current palm pose")
placeresult.manipulation_result.value = ManipulationResult.ERROR
return placeresult
current_pose_= PoseStamped()
current_pose_.header.frame_id = "/world"
current_pose_.pose.position = Point(trans[0],trans[1],trans[2])
current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3])
# for each place location
for index, target_pose_ in enumerate(place_goal.place_locations):
#compute straight trajectory to approach distance
target_approach_pose_= PoseStamped()
target_approach_pose_.header.frame_id = "/world"
target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance)
target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test
# for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to approach pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose")
#print interpolated_motion_plan_res
# check and plan motion to this approach pose
motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach)
#if this approach pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Place pose number "+str(index)+" is possible, executing it")
# copy the pose to execute for the following steps
target_pose_to_execute_ = copy.deepcopy(target_pose_)
break
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach approach pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#put hand in pre-grasp posture (to gently release)
if self.pre_grasp_exec(place_goal.grasp)<0:
#QMessageBox.warning(self, "Warning",
# "Release action failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#detach the object from the hand
rospy.loginfo("Now we detach the attached object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attached object to be detached published")
else:
rospy.logerr("None of the place pose tested is possible")
placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return placeresult
placeresult.manipulation_result.value = ManipulationResult.SUCCESS
placeresult.place_location= target_pose_to_execute_
return placeresult
def poll(self):
self.send_debug_pid()
time_now = rospy.Time.now()
if time_now > self.t_next:
#rospy.logwarn("Voltage: %f, Current: %f", float(vol), float(current))
#vol = self.arduino.detect_voltage()
#current = self.arduino.detect_current()
#rospy.logwarn("Voltage: %f, Current: %f", vol, current)
# Read the encoders
try:
aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts(
)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
#rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))
# Calculate odometry
dist_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
dist_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
dist_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter
# save current encoder value for next calculate
self.enc_A = aWheel_enc
self.enc_B = bWheel_enc
self.enc_C = cWheel_enc
dt = time_now - self.then
self.then = time_now
dt = dt.to_sec()
va = dist_A / dt
vb = dist_B / dt
vc = dist_C / dt
#forward kinemation
vx = sqrt(3) * (va - vb) / 3.0
vy = (va + vb - 2 * vc) / 3.0
vth = (va + vb + vc) / (3 * self.wheel_track)
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# create the odometry transform frame broadcaster.
#self.odomBroadcaster.sendTransform(
# (self.x, self.y, 0),
# (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
# rospy.Time.now(),
# self.base_frame,
# self.odom_name
#)
odom = Odometry()
odom.header.frame_id = self.odom_name
odom.child_frame_id = self.base_frame
odom.header.stamp = time_now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odom.twist.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
self.odomPub.publish(odom)
# send motor cmd
if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
self.stop()
else:
self.send_motor_speed()
# set next compare time
self.t_next = time_now + self.t_delta
#self.battery_monitor_status_msg.dc_voltage = float('nan')
#self.battery_monitor_status_msg.load = float('nan')
#self.battery_monitor_status_msg.percentage = float('nan')
#self.battery_monitor_status_msg.temperature = float('nan')
# self.battery_monitor_status_msg.on = False # as we cannot process the response, we can assume that it is on an error state, so it cannot be considered to be on
'''
def writeToSerialDevice(self, data):
bytes_written = 0
try:
bytes_written = self.serial_device.write(data)
except serial.serialutil.SerialException, e:
rospy.logerr('%s:writeToSerialDevice: Error writing port %s'%(self.node_name, e))
return -1
except Exception, e:
rospy.logerr('%s:writeToSerialDevice: Error writing port %s'%(self.node_name, e))
return -1
return bytes_written
def readFromSerialDevice(self, size):
data_read = ''
try:
data_read = self.serial_device.read(size)
except serial.serialutil.SerialException, e:
rospy.logerr('%s:readFromSerialDevice: Error reading port %s'%(self.node_name, e))
return -1,''
except Exception, e:
rospy.logerr('%s:readFromSerialDevice: Error reading port %s'%(self.node_name, e))
return -1,''
rospy.logwarn("# episode cumulated_reward=>" + str(cumulated_reward))
rospy.logwarn("# State in which we will start next step=>" + str(nextState))
qlearn.learn(state, action, reward, nextState)
if not (done):
rospy.logwarn("NOT DONE")
state = nextState
else:
rospy.logwarn("DONE")
last_time_steps = numpy.append(last_time_steps, [int(i + 1)])
break
rospy.logwarn("############### END Step=>" + str(i))
#raw_input("Next Step...PRESS KEY")
# rospy.sleep(2.0)
m, s = divmod(int(time.time() - start_time), 60)
h, m = divmod(m, 60)
rospy.logerr(("EP: " + str(x + 1) + " - [alpha: " + str(round(qlearn.alpha, 2)) + " - gamma: " + str(
round(qlearn.gamma, 2)) + " - epsilon: " + str(round(qlearn.epsilon, 2)) + "] - Reward: " + str(
cumulated_reward) + " Time: %d:%02d:%02d" % (h, m, s)))
rospy.loginfo(("\n|" + str(nepisodes) + "|" + str(qlearn.alpha) + "|" + str(qlearn.gamma) + "|" + str(
initial_epsilon) + "*" + str(epsilon_discount) + "|" + str(highest_reward) + "| PICTURE |"))
l = last_time_steps.tolist()
l.sort()
# print("Parameters: a="+str)
rospy.loginfo("Overall score: {:0.2f}".format(last_time_steps.mean()))
rospy.loginfo("Best 100 score: {:0.2f}".format(reduce(lambda x, y: x + y, l[-100:]) / len(l[-100:])))
env.close()
def AnalogReadHandler(self, req):
value = self.controller.analog_read(req.pin)
return AnalogReadResponse(value)
def shutdown(self):
rospy.loginfo("Shutting down Arduino Node...")
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.loginfo("Serial port closed.")
os._exit(0)
if __name__ == '__main__':
try:
myArduino = ArduinoROS()
except SerialException:
rospy.logerr("Serial exception trying to open port.")
os._exit(0)
def messageCallback(self, msg, topic):
try:
self.publishers[self.remap(topic)].publish(msg)
except Exception as exc:
rospy.logerr('Error on publishing to {}: {}'.format(topic, exc))
self.activate_all_output()
config = deepcopy(self.component_config_)
data = deepcopy(self.component_data_)
self.set_all_input_read()
self.component_implementation_.update(data, config)
{ifpublisher}
try:
{forallpublisher}
self.component_data_.out_{name}_active = data.out_{name}_active
self.component_data_.out_{name} = data.out_{name}
if self.component_data_.out_{name}_active:
self.{name}_.publish(self.component_data_.out_{name})
{endforallpublisher}
except rospy.ROSException as error:
rospy.logerr("Exception: {}".format(error))
{endifpublisher}
def main():
"""
@brief Entry point of the package.
Instanciate the node interface containing the Developer implementation
@return nothing
"""
rospy.init_node("{nodeName}", anonymous=False)
node = {apply-capitalized_node_name}ROS()
if not node.configure():
rospy.logfatal("Could not configure the node")
rospy.logfatal("Please check configuration parameters")
def convert_ref_frame(arm_state, ref_frame, ref_frame_obj=Landmark()):
"""Transforms an arm frame to a new ref. frame.
Args:
arm_state (ArmState): The arm state to transform
ref_frame (int): One of ArmState.*, the desired reference frame to
transform into.
ref_frame_obj (Landmark): The landmark to transform relative to.
Returns:
ArmState: A copy of arm_state, but transformed.
"""
output_state = copy.deepcopy(arm_state)
ref_frame_obj_copy = copy.deepcopy(ref_frame_obj)
if ref_frame == ArmState.ROBOT_BASE:
if arm_state.refFrame == ArmState.ROBOT_BASE:
pass # Nothing to do
elif arm_state.refFrame == ArmState.OBJECT:
# Transform from object to robot base.
ee_in_obj = get_matrix_from_pose(arm_state.ee_pose)
obj_pose = arm_state.refFrameLandmark.pose # In base frame
obj_to_base = get_matrix_from_pose(obj_pose)
abs_ee_pose = get_pose_from_transform(
np.dot(obj_to_base, ee_in_obj))
output_state.ee_pose = abs_ee_pose
output_state.refFrame = ArmState.ROBOT_BASE
output_state.refFrameLandmark = Landmark()
else:
rospy.logerr(
'Unhandled reference frame conversion: {} to {}'.format(
arm_state.refFrame, ref_frame))
elif ref_frame == ArmState.OBJECT:
if arm_state.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to provided object.
arm_in_base = get_matrix_from_pose(arm_state.ee_pose)
base_to_obj = np.linalg.inv(
get_matrix_from_pose(ref_frame_obj.pose))
rel_ee_pose = get_pose_from_transform(
np.dot(base_to_obj, arm_in_base))
output_state.ee_pose = rel_ee_pose
output_state.refFrame = ArmState.OBJECT
output_state.refFrameLandmark = ref_frame_obj_copy
elif arm_state.refFrame == ArmState.OBJECT:
if arm_state.refFrameLandmark.name == ref_frame_obj.name:
pass # Nothing to do
else:
# Transform from arm state's object to provided object.
ee_in_source_obj = get_matrix_from_pose(arm_state.ee_pose)
source_obj_to_base = get_matrix_from_pose(
arm_state.refFrameLandmark.pose)
base_to_target_obj = np.linalg.inv(
get_matrix_from_pose(ref_frame_obj.pose))
rel_ee_pose = get_pose_from_transform(
np.dot(np.dot(base_to_target_obj, source_obj_to_base),
ee_in_source_obj))
output_state.ee_pose = rel_ee_pose
output_state.refFrame = ArmState.OBJECT
output_state.refFrameLandmark = ref_frame_obj_copy
else:
rospy.logerr(
'Unhandled reference frame conversion: {} to {}'.format(
arm_state.refFrame, ref_frame))
return output_state
goal_lifting.trajectory.joint_names.append('joint_lifting')
temp_joint_lifting=JointTrajectoryPoint()
temp_joint_lifting.positions=[goal_joint_position[0]]
temp_joint_lifting.accelerations=[0.0]
temp_joint_lifting.velocities=[0.0]
temp_joint_lifting.effort=[0.0]
temp_joint_lifting.time_from_start=rospy.Duration(secs=2.0)
goal_lifting.trajectory.points.append(temp_joint_lifting)
goal_lifting.trajectory.header.stamp=rospy.Time.now()+rospy.Duration(secs=1)
client_lifting.send_goal(goal_lifting)
flag = 2
#client_lifting.wait_for_result(rospy.Duration.from_sec(2.0))
def get_key(self):
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.setting)
return key
if __name__ == '__main__':
try:
KeyboardTeleopRvizArm()
except rospy.ROSInterruptException:
rospy.logerr("ROS Error!")
# Note that TopicDiagnostic, HeaderlessDiagnosedPublisher,
# HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from
# CompositeDiagnosticTask, so you can add your own fields to them using
# the addTask method.
#
# Each time pub1_freq is updated, lower will also get updated and its
# output will be merged with the output from pub1_freq.
pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful).
# If we know that the state of the node just changed, we can force an
# immediate update.
updater.force_update()
# We can remove a task by refering to its name.
if not updater.removeByName("Bound check"):
rospy.logerr("The Bound check task was not found when trying to remove it.")
while not rospy.is_shutdown():
msg = std_msgs.msg.Bool()
rospy.sleep(0.1)
# Calls to pub1 have to be accompanied by calls to pub1_freq to keep
# the statistics up to date.
msg.data = False
pub1.publish(msg)
pub1_freq.tick()
# We can call updater.update whenever is convenient. It will take care
# of rate-limiting the updates.
updater.update()
def update_object_pose(self):
""" Function to externally update an object pose."""
# Look down at the table.
rospy.loginfo('Head attempting to look at table.')
Response.force_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE):
rospy.sleep(PAUSE_SECONDS)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Head is now (successfully) staring at table.')
try:
resp = self._segment_tabletop()
rospy.loginfo("Adding landmarks")
self._obj_sides = []
self._reset_objects()
# add the table
xmin = resp.table.x_min
ymin = resp.table.y_min
xmax = resp.table.x_max
ymax = resp.table.y_max
depth = xmax - xmin
width = ymax - ymin
pose = resp.table.pose.pose
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self._surface = _get_surface_marker(pose, dimensions)
self._im_server.insert(self._surface, self.marker_feedback_cb)
self._im_server.applyChanges()
for cluster in resp.clusters:
points = cluster.points
if (len(points) == 0):
return Point(0, 0, 0)
[minX, maxX, minY, maxY, minZ, maxZ] = [
points[0].x, points[0].x, points[0].y, points[0].y,
points[0].z, points[0].z
]
for pt in points:
minX = min(minX, pt.x)
minY = min(minY, pt.y)
minZ = min(minZ, pt.z)
maxX = max(maxX, pt.x)
maxY = max(maxY, pt.y)
maxZ = max(maxZ, pt.z)
object_sides_list = {
'minX': minX,
'minY': minY,
'minZ': minZ,
'maxX': maxX,
'maxY': maxY,
'maxZ': maxZ
}
self._obj_sides += [object_sides_list]
self._add_new_object(
Pose(
Point((minX + maxX) / 2, (minY + maxY) / 2,
(minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
Point(maxX - minX, maxY - minY, maxZ - minZ), False)
return True
except rospy.ServiceException, e:
print "Call to segmentation service failed: %s" % e
return False
def algorithm_setup(self):
rospy.sleep(2)
rospy.loginfo("Algorithm setup initiated: TPBP ")
rospy.loginfo("%s, %s, %s, %s, %s, %s, %s, %s", self.dirname,
self.folder, self.num_robots, self.priority_nodes,
self.graph_name, self.algo, self.algo_params,
self.time_periods)
print("Inside algo_setup Robot ids: " + str(self.robot_ids))
if self.algo != "tpbp_walk":
rospy.logwarn(" Algorithm is not TPBP_WALK. Received algo is <%s>",
self.algo)
return
self.dest_folder = self.dirname + '/processing/' + self.folder
self.graph = nx.read_graphml(self.dirname + '/graph_ml/' +
self.graph_name + '.graphml')
for node in self.graph.nodes():
self.graph.node[node]['idleness'] = 0.
rospy.loginfo_throttle(1, ' node idleness set to 0')
self.stamp = 0.0
self.assigned = []
for i in self.priority_nodes:
self.assigned.append(False)
self.robot_cur_walks = {}
self.task_list = {}
#self.task_done_subs = rospy.Subscriber('/task_done', TaskDone, self.task_done_CB)
self.tpbp_service = rospy.Service('pmm_to_tpbp', TaskDoneSrv,
self.task_done_CB)
rospy.logdebug(" Constructing TPBP _line_=91 ")
for index, ith_robot in zip(range(len(self.robot_ids)),
self.robot_ids):
rospy.loginfo("Computing initial tpbp_walk for robot %s",
ith_robot)
time_start = time.clock()
self.robot_cur_walks[ith_robot] = tpbp_walk(
self.graph, self.algo_params, self.priority_nodes[index],
self.priority_nodes, self.time_periods, self.assigned,
self.dest_folder)
print(self.robot_cur_walks)
rospy.loginfo('PRINTING Dict')
rospy.loginfo(self.robot_cur_walks)
self.assigned[self.priority_nodes.index(
self.robot_cur_walks[ith_robot][-1])] = True
time_end = time.clock()
time_spend = time_end - time_start
rospy.loginfo(" Completed computation of tpbp_walk. Time spent %f",
time_spend)
#self.task_list.append(map(int, self.robot_cur_walks[i]))
rospy.loginfo("TASK_LIST PRINT:")
self.task_list[ith_robot] = map(str,
self.robot_cur_walks[ith_robot])
rospy.loginfo(self.task_list)
rospy.loginfo(self.robot_cur_walks)
rospy.loginfo("Line _163_ Robot ids: %s", str(self.robot_ids))
for ith_robot in self.robot_ids:
rospy.loginfo("inside for of robot id %s", ith_robot)
try:
rospy.wait_for_service('tpbp_to_pmm')
self.tpbp_to_pmm_proxy = rospy.ServiceProxy(
'tpbp_to_pmm', NextTaskSrv)
try:
print(
self.tpbp_to_pmm_proxy(self.task_list[ith_robot],
ith_robot))
rospy.loginfo(self.task_list[ith_robot])
rospy.loginfo(ith_robot)
except Exception, e:
rospy.logwarn(
" Not able to publish the next task for robot %s Exception is:",
ith_robot)
rospy.logerr(e)
except Exception, e:
rospy.logerr(e)
rospy.logerr(
"unable to create service client for robot %s for topic %s",
ith_robot)
#!/usr/bin/env python3
import time
import rospy
from std_msgs.msg import String
if __name__ == '__main__':
try:
pub = rospy.Publisher('/move/cmd_str', String, queue_size=10)
rospy.init_node('circle')
rate = rospy.Rate(1) # 1hz
now = time.time()
future = now + 5
while time.time() < future and not rospy.is_shutdown():
rospy.loginfo("Turning Left.")
pub.publish("left")
pub.publish("stop")
except Exception as e:
rospy.logerr("An error occurred")
limbs= [None,None]
limbs[RIGHT]= baxter_interface.Limb(LRTostr(RIGHT))
limbs[LEFT]= baxter_interface.Limb(LRTostr(LEFT))
joint_names= [[],[]]
#joint_names[RIGHT]= ['right_'+joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
#joint_names[LEFT]= ['left_' +joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
joint_names[RIGHT]= limbs[RIGHT].joint_names()
joint_names[LEFT]= limbs[LEFT].joint_names()
client= actionlib.SimpleActionClient('/robot/limb/%s/follow_joint_trajectory'%LRTostr(arm), control_msgs.msg.FollowJointTrajectoryAction)
# Wait some seconds for the head action server to start or exit
if not client.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Exiting - Joint Trajectory Action Server Not Found')
rospy.logerr('Run: rosrun baxter_interface joint_trajectory_action_server.py')
rospy.signal_shutdown('Action Server not found')
sys.exit(1)
goal= control_msgs.msg.FollowJointTrajectoryGoal()
goal.goal_time_tolerance= rospy.Time(0.1)
goal.trajectory.joint_names= joint_names[arm]
def add_point(goal, time, positions):
point= trajectory_msgs.msg.JointTrajectoryPoint()
point.positions= copy.deepcopy(positions)
point.time_from_start= rospy.Duration(time)
goal.trajectory.points.append(point)
angles= limbs[arm].joint_angles()
add_point(goal, 0.0, [angles[joint] for joint in joint_names[arm]])
def joyCB(self, status):
# a callback function
rospy.logerr("%s: no joyCB is overriden" % (self.name))
def close_gripper(self):
try:
self.gripper.grasp(-0.1)
except:
rospy.logerr('grasp close error')
def get_cur_pos():
try:
return rospy.wait_for_message("/rv7f/joint_states", JointState, 5.0)
except (rospy.ROSException, rospy.ROSInterruptException):
rospy.logerr('Unable to read current position')
raise
def open_gripper(self):
try:
self.gripper.command(1.5)
except:
rospy.logerr('grasp open error')
def half_gripper(self):
try:
self.gripper.command(0.6)
except:
rospy.logerr('grasp close error')
