Skip to content

Commit

Permalink
fix pylint errors
Browse files Browse the repository at this point in the history
  • Loading branch information
fmessmer committed Jan 30, 2020
1 parent 2ba646e commit 551e5b3
Show file tree
Hide file tree
Showing 8 changed files with 31 additions and 27 deletions.
2 changes: 1 addition & 1 deletion cob_grasp_generation/scripts/show_grasps_or_client.py
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,6 @@ def show_grasps_client():
if __name__ == '__main__':
try:
rospy.init_node('show_grasp_client')
result = show_grasps_client()
show_grasps_client()
except rospy.ROSInterruptException:
print("program interrupted before completion")
2 changes: 1 addition & 1 deletion cob_grasp_generation/scripts/show_grasps_rviz_client.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,6 @@ def show_grasps_client():
if __name__ == '__main__':
try:
rospy.init_node('show_grasp_client')
result = show_grasps_client()
show_grasps_client()
except rospy.ROSInterruptException:
print("program interrupted before completion")
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@

"""Analyze Grasp 3D - own module: a collection of several function for analyzing the grasps
"""
from openravepy import *

from openravepy import poseFromMatrix # pylint: disable=import-error
import numpy, time, scipy, csv, os
import roslib.packages

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
from itertools import groupby
import numpy, time, scipy
import roslib
from openravepy import *
from openravepy import Environment # pylint: disable=import-error
from openravepy import databases, options, matrixFromPose # pylint: disable=import-error
from cob_grasp_generation import analyzegrasp3d, grasp_query_utils


Expand Down
6 changes: 3 additions & 3 deletions cob_moveit_interface/scripts/load_scene_example.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,9 @@


import rospy
from tf.transformations import *
from simple_moveit_interface import *
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler
from simple_moveit_interface import get_planning_scene_interface, add_ground, clear_objects


def gen_pose(frame_id="/odom_combined", pos=[0,0,0], euler=[0,0,0]):
Expand Down Expand Up @@ -81,6 +82,5 @@ def gen_pose(frame_id="/odom_combined", pos=[0,0,0], euler=[0,0,0]):
psi.remove_world_object("sphere")

### CLEAR ALL OBJECTS
#ToDo: attached objects are not removed
clear_objects()

Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@
def get_transform_listener():
'''
Gets the transform listener for this process.
This is needed because tf only allows one transform listener per process. Threadsafe, so
that two threads could call this at the same time, at it will do the right thing.
'''
Expand All @@ -54,23 +53,22 @@ def get_transform_listener():
def get_move_group_commander(group):
'''
Gets the move_group_commander for this process.
'''
global _mgc_dict
with _mgc_dict_creation_lock:
if not group in _mgc_dict:
_mgc_group = MoveGroupCommander(group)
_mgc_group.set_planner_id('RRTConnectkConfigDefault')
_mgc_dict[group] = _mgc_group
add_ground()
return _mgc_dict[group]

add_ground()
return _mgc_dict[group]



def get_planning_scene_interface():
'''
Gets the planning_scene_interface for this process.
'''
global _psi
with _psi_creation_lock:
Expand All @@ -94,12 +92,12 @@ def add_ground():
pose.header.frame_id = "base_link"
psi.attach_box("base_link", "ground", pose, (3, 3, 0.1))

def clear_objects(attach_link):
#ToDo: attached objects are not removed
def clear_objects():
psi = get_planning_scene_interface()
psi.remove_attached_object(attach_link)
psi.remove_world_object("")

def clear_attached_object(attach_link, object_name):
def clear_attached_object(attach_link, object_name=None):
psi = get_planning_scene_interface()
psi.remove_attached_object(link = attach_link, name = object_name)
psi.remove_world_object(object_name)
Expand Down Expand Up @@ -152,11 +150,11 @@ def moveit_cart_goals(group, ref_frame, goal_list, avoid_collisions=True):

if frac == 1.0:
if mgc.execute(traj):
print("Done moving")
return 'succeeded'
else:
print("Something happened during execution")
print('failed')
print("Done moving")
return 'succeeded'
else:
print("Something happened during execution")
return 'failed'
else:
print("Failed to plan full path!")
return 'failed'
Expand All @@ -180,7 +178,6 @@ def moveit_get_current_pose(group):
####################################

def get_goal_from_server(group, parameter_name):

ns_global_prefix = "/script_server"

# get joint_names from parameter server
Expand Down Expand Up @@ -219,7 +216,6 @@ def get_goal_from_server(group, parameter_name):
print("parameter is:",param)
return None


#no need for trajectories anymore, since planning (will) guarantee collision-free motion!
point = param[len(param)-1]

Expand Down
10 changes: 7 additions & 3 deletions cob_pick_place_action/scripts/cob_multi_pick_place_client.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,10 @@
from math import pi

import rospy
import roslib
import actionlib
from geometry_msgs.msg import PoseStamped
from tf.transformations import *
from tf.transformations import quaternion_from_euler
import simple_moveit_interface as smi_
import cob_pick_place_action.msg

Expand All @@ -39,8 +40,11 @@ def setup_environment():
psi = smi_.get_planning_scene_interface()
rospy.sleep(1.0)

#smi_.clear_objects("arm_7_link")
smi_.clear_objects("arm_left_7_link")

#smi_.clear_attached_object("arm_7_link")
smi_.clear_attached_object("arm_left_7_link")
#smi_.clear_objects()
smi_.clear_objects()

### Add a floor
smi_.add_ground()
Expand Down
6 changes: 4 additions & 2 deletions cob_pick_place_action/scripts/cob_pick_action_client.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,10 @@ def setup_environment():
psi = smi_.get_planning_scene_interface()
rospy.sleep(1.0)

#smi_.clear_objects("arm_7_link")
smi_.clear_objects("arm_left_7_link")
#smi_.clear_attached_object("arm_7_link")
smi_.clear_attached_object("arm_left_7_link")
#smi_.clear_objects()
smi_.clear_objects()

### Add a floor
smi_.add_ground()
Expand Down

0 comments on commit 551e5b3

Please sign in to comment.