Rospy script to replicate chomp obstacle error.

faunc_chomp_error.py

#!/usr/bin/env python

import rospy
import std_msgs.msg
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
import geometry_msgs.msg
import time

def load_scene():
    scene = PlanningSceneInterface()

    # clear the scene
    scene.remove_world_object()

    robot = RobotCommander()

    # pause to wait for rviz to load
    rospy.sleep(4)

    p = geometry_msgs.msg.PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.header.stamp = rospy.Time.now()
    p.pose.position.x = 0.68
    p.pose.position.y = 0.3
    p.pose.position.z = 1.25
    p.pose.orientation.x = 0.0
    p.pose.orientation.y = 0.0
    p.pose.orientation.z = 0.0
    p.pose.orientation.w = 1.0
    scene.add_box('box', p, (0.15, 0.15, 0.15))
    rospy.sleep(0.2)

def make_plan():
    group = MoveGroupCommander('manipulator')
    joint_state = JointState()
    joint_state.header = std_msgs.msg.Header()
    joint_state.header.stamp = rospy.Time.now()
    joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
    joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    moveit_robot_state = RobotState()
    moveit_robot_state.joint_state = joint_state
    group.set_start_state(moveit_robot_state)
    current_joints = group.get_current_joint_values()
    current_joints[0] = 1.0
    group.set_joint_value_target(current_joints)
    plan = group.plan()
    rospy.sleep(5)
    

if __name__ == '__main__':
    rospy.init_node('test_chomp', anonymous=True)
    while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown():
        time.sleep(0.5)
    load_scene()
    make_plan()
rospy.spin()