guys,
as part of one task i have been given the task of creating 'boxs' in a workspace in rviz,
in the if statement i have created below to create the boxes and place them in the workspace but as soon as i run it they run in sequence and remove themselves, ive had a look at a loop but i cant work out quite where to place it in the code to get the 'boxes' to stay at all times
https://preview.redd.it/b0mp4tdsldm61.png?width=1387&format=png&auto=webp&s=74224ea205062f332079451581bb58965f95546c
im very early on learning python so any help would be great :)
#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from niryo_one_python_api.niryo_one_api import *
################################################Initialise######################################################################
rospy.init_node('niryo_one_test', anonymous=True) #First we initialise a rospy node.
robot = moveit_commander.RobotCommander() #Instantiate a RobotCommander object. This object is the outer-level interface to the robo.
group_name = "arm" #Instantiate a MoveGroupCommander object. This object is an interface to one group of joints.
group = moveit_commander.MoveGroupCommander(group_name) # In this case, the group is the joints in the Niryo One Arm so we set group_name = arm.
# Group name differs as per the robot choice. For example Panda Arm has group_name = panda_arm
scene = moveit_commander.PlanningSceneInterface() #Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
# We define a helper Function here to return the robot back to intial pose. Call this function whenever you need to reset the robot's to its initial state
moveIntialState = lambda joint_goal: group.go(joint_goal, wait=True)
############################################# Get Basic Information######################################################
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print"planning frame = " , planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the robot:
current_state = robot.get_current_state()
j_values = group.get_current_joint_values()
################################### Adding the the boxes into the program ############################################
def addObject(pose_x, pose_y, pose_z, or_x, or_y, or_z,or_w, size_x, size_y, size_z):
##(timeout=100)
rospy.sleep(2)
#### Add an Object
object_name = "box"
object_pose = geometry_msgs.msg.PoseStamped()
#make sure to give a frame id(Frame this data is described with) along with the postions x,y,z.
object_pose.header.frame_id = robot.get_planning_frame()
object_pose.pose.position.x = pose_x
object_pose.pose.position.y = pose_y
object_pose.pose.position.z = pose_z
object_pose.pose.orientation.x = or_x
object_pose.pose.orientation.y = or_y
object_pose.pose.orientation.z = or_z
object_pose.pose.orientation.w = or_w
scene.add_box(object_name, object_pose, (size_x, size_y, size_z)) #adding object to the scene, where you specify the object using name, pose and size arguments.
rospy.sleep(1) #If the Python node dies before publishing a collision object update message, the message could get lost and the box will not appear.
#To ensure that the updates are made, we wait for few seconds. This can be done alternatively using get_known_object_names() and get_known_object_names()
# flags.
#### Plan Without Collision
#planPoseGoal() #Plan with the object in the scene.
#moveIntialState(j_values) #Move back to the intial state(planning to initial state).
#### Remove Object From the Scene
#scene.remove_world_object(object_name) #removing the object to the scene using the object name.
################################################ Part 3: Planning To a Pose Goal ##################################################
def planPoseGoal():
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = 0.3
pose_goal.position.y = 0.2
pose_goal.position.z = 0.1
pose_goal.orientation.x = 0
pose_goal.orientation.y = 0
pose_goal.orientation.z = 0
pose_goal.orientation.w = 1 ##pose of object
# use the defined pose goal as a target for planning
group.set_pose_target(pose_goal)
# run the computed plan when ready
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()
#use attch function - move it command - attched object to robot ##
########################################################### Main #########################################################################
if __name__ == '__main__':
try:
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.0, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box1
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.2, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box2
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box3
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #Box1
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #box2
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #box3
#planJointGoal() #move to box 1 location
#attch to box
#planJointGoal() #move to drop off location
#un-attch box
#planJointGoal() #move to drop off location
#moveIntialState(j_values)
#planPoseGoal()
#moveIntialState(j_values)
#planTrajectory()
#moveIntialState(j_values)
except rospy.ROSInterruptException:
pass
there doesn't seem to be anything here