diff --git a/CMakeLists.txt b/CMakeLists.txt index ea877b8..31beb6b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(myCobotROS) +project(mycobot_ros) add_compile_options(-std=c++11) ## Find catkin and any catkin packages @@ -9,7 +9,10 @@ find_package(catkin REQUIRED COMPONENTS std_msgs genmsg message_generation - serial + actionlib + moveit_msgs + image_transport + cv_bridge ) add_message_files(FILES @@ -28,21 +31,33 @@ add_service_files(FILES generate_messages(DEPENDENCIES std_msgs) ## Declare a catkin package -catkin_package(CATKIN_DEPENDS message_runtime std_msgs) +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs actionlib moveit_msgs +) ## Build talker and listener -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) catkin_install_python(PROGRAMS - scripts/display.py - scripts/control_slider.py + scripts/follow_display.py + scripts/slider_control.py scripts/control_marker.py - scripts/mycobot_ros.py + scripts/mycobot_services.py scripts/listen_real.py scripts/teleop_keyboard.py scripts/client.py + scripts/camera.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +find_package(OpenCV REQUIRED) +add_executable(opencv_camera src/opencv_camera) +target_link_libraries(opencv_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_executable(camera_display src/camera_display) +target_link_libraries(camera_display ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) diff --git a/README.md b/README.md index 113bcf6..eebcdc0 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# myCobotROS +# mycobot_ros [![jaywcjlove/sb](https://jaywcjlove.github.io/sb/lang/chinese.svg)](READMEcn.md) @@ -46,7 +46,7 @@ This command does three things: 3) `docker-compose up ros` This runs the image specified in the `docker-compose.yml`, which by default runs - the command `roslaunch myCobotROS control_slider.launch` within the container. + the command `roslaunch mycobot_ros control_slider.launch` within the container. ### Option 2: Local @@ -64,7 +64,7 @@ Install ros package in your src folder of your Catkin workspace. ```bash $ cd ~/catkin_ws/src -$ git clone https://github.com/elephantrobotics/myCobotROS.git +$ git clone https://github.com/elephantrobotics/mycobot_ros.git $ cd ~/catkin_ws $ catkin_make $ source ~/catkin_ws/devel/setup.bash @@ -73,7 +73,7 @@ $ source ~/catkin_ws/devel/setup.bash #### 1.3 Test Python API ```bash -cd ~/catkin_ws/src/myCobotROS +cd ~/catkin_ws/src/mycobot_ros python scripts/test.py ``` @@ -109,13 +109,13 @@ python scripts/test.py - launch ros and rviz ``` - roslaunch myCobotROS control_slider.launch + roslaunch mycobot_ros control_slider.launch ``` - run python script ``` - rosrun myCobotROS control_slider.py + rosrun mycobot_ros control_slider.py ``` - **The model moves with the real manipulator** @@ -123,13 +123,13 @@ python scripts/test.py - launch ros and rviz ``` - roslanuch myCobotROS mycobot.launch + roslanuch mycobot_ros mycobot.launch ``` - run python script ``` - rosrun myCobotROS display.py + rosrun mycobot_ros display.py ``` - **Open the keyboard controller** @@ -137,13 +137,13 @@ python scripts/test.py - launch ``` - roslaunch myCobotROS mycobot_teleop_keyboard.launch PORT:=/dev/ttyUSB0 + roslaunch mycobot_ros mycobot_teleop_keyboard.launch PORT:=/dev/ttyUSB0 ``` - open another terminal run script ``` - rosrun myCobotROS teleop_keyboard.py _speed:=60 _change_size:=10 + rosrun mycobot_ros teleop_keyboard.py _speed:=60 _change_size:=10 ``` Then you will see this: @@ -155,6 +155,8 @@ python scripts/test.py w(x+) a(y-) s(x-) d(y+) + + z(z-) x(z+) u(rx+) i(ry+) o(rz+) j(rx-) k(ry-) l(rz-) @@ -172,10 +174,24 @@ python scripts/test.py currently: speed 50 change size 10 ``` +## MoveIT + +### Execute plan with actual robot. + +``` +roslaunch mycobot_ros demo.launch +``` + +Open a new terminal and run: + +``` +rosrun mycobot_ros sync_signal.py +``` + ## Q & A diff --git a/READMEcn.md b/READMEcn.md index 3bc9497..62d5f65 100644 --- a/READMEcn.md +++ b/READMEcn.md @@ -1,4 +1,4 @@ -# myCobotROS +# mycobot_ros [![jaywcjlove/sb](https://jaywcjlove.github.io/sb/lang/english.svg)](README.md) @@ -33,7 +33,7 @@ pip install pymycobot --user ```bash $ cd ~/catkin_ws/src -$ git clone https://github.com/elephantrobotics/myCobotROS.git +$ git clone https://github.com/elephantrobotics/mycobot_ros.git $ cd ~/catkin_ws $ catkin_make ``` @@ -41,7 +41,7 @@ $ catkin_make ### 1.3 你可以选择测试 Python API ```bash -cd ~/catkin_ws/src/myCobotROS +cd ~/catkin_ws/src/mycobot_ros python scripts/test.py ``` @@ -77,13 +77,13 @@ python scripts/test.py - 启动 ros 和 rviz ``` - roslaunch myCobotROS control_slider.launch + roslaunch mycobot_ros control_slider.launch ``` - 运行 python 脚本 ``` - rosrun myCobotROS control_slider.py + rosrun mycobot_ros control_slider.py ``` - 仿真模型同步机械臂 @@ -91,13 +91,13 @@ python scripts/test.py - 启动 ros 和 rviz ``` - roslanuch myCobotROS mycobot.launch + roslanuch mycobot_ros mycobot.launch ``` - 运行 python 脚本 ``` - rosrun myCobotROS display.py + rosrun mycobot_ros display.py ``` ## Q & A diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/config/fake_controllers.yaml b/config/fake_controllers.yaml new file mode 100644 index 0000000..512725e --- /dev/null +++ b/config/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: fake_mycobot_arm_controller + joints: + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_joint6 \ No newline at end of file diff --git a/config/firefighter.srdf b/config/firefighter.srdf new file mode 100644 index 0000000..37f1d40 --- /dev/null +++ b/config/firefighter.srdf @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/config/joint_limits.yaml b/config/joint_limits.yaml new file mode 100644 index 0000000..6e9612a --- /dev/null +++ b/config/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint2_to_joint1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint3_to_joint2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint4_to_joint3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint5_to_joint4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6_to_joint5: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6output_to_joint6: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/config/kinematics.yaml b/config/kinematics.yaml new file mode 100644 index 0000000..0290bbd --- /dev/null +++ b/config/kinematics.yaml @@ -0,0 +1,5 @@ +mycobot_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/config/ompl_planning.yaml b/config/ompl_planning.yaml new file mode 100644 index 0000000..acdfcd2 --- /dev/null +++ b/config/ompl_planning.yaml @@ -0,0 +1,148 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +mycobot_arm: + default_planner_config: RRT + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml new file mode 100644 index 0000000..b7aaea5 --- /dev/null +++ b/config/ros_controllers.yaml @@ -0,0 +1,35 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: mycobot_arm + joint_model_group_pose: init_pose +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_joint6 + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: mycobot_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_joint6 \ No newline at end of file diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml new file mode 100644 index 0000000..d2955dc --- /dev/null +++ b/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/docker-compose.yml b/docker-compose.yml index e11f3fb..28da39c 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,7 +1,7 @@ version: '3.4' x-app: &common - command: [ "roslaunch myCobotROS control_slider.launch" ] + command: [ "roslaunch mycobot_ros control_slider.launch" ] privileged: true environment: PYTHONUNBUFFERED: 1 diff --git a/launch/chomp_planning_pipeline.launch.xml b/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..06a3d4d --- /dev/null +++ b/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/communication_service.launch b/launch/communication_service.launch index 7cfdf62..363568f 100644 --- a/launch/communication_service.launch +++ b/launch/communication_service.launch @@ -3,7 +3,7 @@ - + diff --git a/launch/default_warehouse_db.launch b/launch/default_warehouse_db.launch new file mode 100644 index 0000000..d794ae3 --- /dev/null +++ b/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/demo.launch b/launch/demo.launch new file mode 100644 index 0000000..6e9e240 --- /dev/null +++ b/launch/demo.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch new file mode 100644 index 0000000..903d5a2 --- /dev/null +++ b/launch/demo_gazebo.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/fake_moveit_controller_manager.launch.xml b/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..ecb363c --- /dev/null +++ b/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/firefighter_moveit_controller_manager.launch.xml b/launch/firefighter_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..8bd4853 --- /dev/null +++ b/launch/firefighter_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/launch/firefighter_moveit_sensor_manager.launch.xml b/launch/firefighter_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/launch/firefighter_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/launch/gazebo.launch b/launch/gazebo.launch new file mode 100644 index 0000000..00d0739 --- /dev/null +++ b/launch/gazebo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/joystick_control.launch b/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/move_group.launch b/launch/move_group.launch new file mode 100644 index 0000000..d801862 --- /dev/null +++ b/launch/move_group.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/moveit.rviz b/launch/moveit.rviz new file mode 100644 index 0000000..38b3736 --- /dev/null +++ b/launch/moveit.rviz @@ -0,0 +1,250 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Geometry1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.742560029 + Tree Height: 325 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.0799999982 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: mycobot_arm + Query Goal State: true + Query Start State: true + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.200000003 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: joint1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 1.39158678 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.113567002 + Y: 0.105920002 + Z: 2.23518001e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.439796686 + Target Frame: joint1 + Value: XYOrbit (rviz) + Yaw: 2.5317657 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c0061007900730100000028000001da000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002080000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 92 + Y: 24 diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch new file mode 100644 index 0000000..b44e3f5 --- /dev/null +++ b/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/launch/mycobot.launch b/launch/mycobot_follow.launch similarity index 71% rename from launch/mycobot.launch rename to launch/mycobot_follow.launch index 75df517..ed7af6f 100644 --- a/launch/mycobot.launch +++ b/launch/mycobot_follow.launch @@ -1,6 +1,6 @@ - - + + diff --git a/launch/control_slider.launch b/launch/mycobot_slider.launch similarity index 59% rename from launch/control_slider.launch rename to launch/mycobot_slider.launch index 82d03c3..1914e8e 100644 --- a/launch/control_slider.launch +++ b/launch/mycobot_slider.launch @@ -1,6 +1,9 @@ - - + + + + @@ -12,7 +15,10 @@ - + diff --git a/launch/mycobot_teleop_keyboard.launch b/launch/mycobot_teleop_keyboard.launch index 66d55ed..6762ad6 100644 --- a/launch/mycobot_teleop_keyboard.launch +++ b/launch/mycobot_teleop_keyboard.launch @@ -2,8 +2,8 @@ - - + + @@ -13,9 +13,9 @@ - + - + diff --git a/launch/ompl_planning_pipeline.launch.xml b/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..6c39d7f --- /dev/null +++ b/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/planning_context.launch b/launch/planning_context.launch new file mode 100644 index 0000000..214e340 --- /dev/null +++ b/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_pipeline.launch.xml b/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..d1e4c20 --- /dev/null +++ b/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch new file mode 100644 index 0000000..255177f --- /dev/null +++ b/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/launch/run_benchmark_ompl.launch b/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..066ed63 --- /dev/null +++ b/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..6d2cd5b --- /dev/null +++ b/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/setup_assistant.launch b/launch/setup_assistant.launch new file mode 100644 index 0000000..d0674f0 --- /dev/null +++ b/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/launch/trajectory_execution.launch.xml b/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..d1a0ab3 --- /dev/null +++ b/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/warehouse.launch b/launch/warehouse.launch new file mode 100644 index 0000000..fbae8fb --- /dev/null +++ b/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/warehouse_settings.launch.xml b/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 90171d2..ac79a27 100644 --- a/package.xml +++ b/package.xml @@ -1,28 +1,44 @@ - myCobotROS + mycobot_ros 0.1.0 - The myCobotROS package + The mycobot_ros package - ZhangLijun - BSD - ZhangLijun + ZhangLijun + + BSD + + catkin + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + joint_state_publisher + robot_state_publisher + xacro + roscpp rospy std_msgs - serial message_generation roscpp rospy std_msgs - serial message_runtime + actionlib + moveit_msgs + + actionlib + moveit_msgs + diff --git a/scripts/camera.py b/scripts/camera.py new file mode 100755 index 0000000..7d3a33a --- /dev/null +++ b/scripts/camera.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import rospy +import cv2 as cv +import numpy as np +from geometry_msgs.msg import Twist +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image +from visualization_msgs.msg import Marker +import tf_conversions +from mycobot_ros.srv import ( + GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) + + +class image_converter: + def __init__(self): + self.mark_pub = rospy.Publisher("/bebop/marker", Marker, queue_size=1) + self.bridge = CvBridge() + self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) + self.aruo_params = cv.aruco.DetectorParameters_create() + calibrationParams = cv.FileStorage('calibrationFileName.xml', cv.FILE_STORAGE_READ) + self.dist_coeffs = calibrationParams.getNode('distCoeffs').mat() + self.camera_matrix = None + # rospy.wait_for_service('get_joint_coords') + # rospy.wait_for_service('set_joint_coords') + + try: + self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords) + self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords) + except: + print('Error: cannot connect service...') + exit(1) + self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) + + + def callback(self, data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + # sucess, cv_image = self.cap.read() + except CvBridgeError as e: + print(e) + size = cv_image.shape + focal_length = size[1] + center = [size[1]/2, size[0]/2] + if self.camera_matrix is None: + self.camera_matrix = np.array([ + [focal_length,0,center[0]], + [0, focal_length, center[1]], + [0,0,1], + ], dtype=np.float32) + gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) + corners, ids, rejectImaPoint = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) + if len(corners) > 0: + if ids is not None: + # print('corners:', corners, 'ids:', ids) + rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(corners, 0.05, self.camera_matrix, self.dist_coeffs) + (rvec - tvec).any() + + print('rvec:', rvec, 'tvec:', tvec) + + for i in range(rvec.shape[0]): + cv.aruco.drawDetectedMarkers(cv_image, corners) + cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03) + + res = self.get_coords() + if res.x == res.y == 0.0: + return + record_coords = [ + res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1 + ] + print(record_coords) + + # [x, y, z, -172, 3, -46.8] + cv.imshow("Image", cv_image) + + + # marker = Marker() + # marker.header.frame_id = '/joint1' + # marker.header.stamp = rospy.Time.now() + # marker.type = marker.SPHERE + # marker.action = marker.ADD + # marker.scale.x = 0.04 + # marker.scale.y = 0.04 + # marker.scale.z = 0.04 + + # marker.pose.position.x = center_x / 100 + # marker.pose.position.y = center_y / 100 + # marker.pose.position.z = 0 + + # marker.color.a = 1.0 + # marker.color.g = 1.0 + + cv.waitKey(3) + try: + # self.mark_pub.publish(marker) + pass + except CvBridgeError as e: + print e +if __name__ == '__main__': + try: + rospy.init_node("cv_bridge_test") + rospy.loginfo("Starting cv_bridge_test node") + image_converter() + rospy.spin() + except KeyboardInterrupt: + print "Shutting down cv_bridge_test node." + cv.destroyAllWindows() \ No newline at end of file diff --git a/scripts/cilibrate_camera.py b/scripts/cilibrate_camera.py new file mode 100644 index 0000000..16b3e7d --- /dev/null +++ b/scripts/cilibrate_camera.py @@ -0,0 +1,31 @@ +import numpy as np +import cv2 +import cv2.aruco as aruco +import glob + +def calibrateKd(im_fpath, aruco_len=60.0): + (w, h) = (6, 4) + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) + objp = np.zeros((w*h, 3), np.float32) + objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) + objp *= aruco_len + objpoints, imgpoints = [], [] + images = glob.glob(f'{im_fpath}/*') + for fname in images: + img = cv2.imread(fname) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + ret, corners = cv2.findChessboardCorners(gray, (w, h), None) + if ret == True: + objpoints.append(objp) + corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + imgpoints.append(corners2) + img = cv2.drawChessboardCorners(img, (6, 4), corners2, ret) + + ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shap[::-1], None, None) + tot_error = 0 + for i in range(len(objpoints)): + imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist) + error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2) + tot_error += error + print(tot_error) + return mtx, dist diff --git a/scripts/client.py b/scripts/client.py index da0cc6b..ec11c81 100755 --- a/scripts/client.py +++ b/scripts/client.py @@ -1,7 +1,7 @@ #!/usr/bin/env python2 import sys import rospy -from myCobotROS.srv import * +from mycobot_ros.srv import * def test(): diff --git a/scripts/display.py b/scripts/follow_display.py similarity index 76% rename from scripts/display.py rename to scripts/follow_display.py index e18c893..238dcea 100755 --- a/scripts/display.py +++ b/scripts/follow_display.py @@ -1,7 +1,6 @@ #!/usr/bin/env python2 # license removed for brevity import time -import subprocess import rospy from sensor_msgs.msg import JointState @@ -12,9 +11,28 @@ from pymycobot.mycobot import MyCobot def talker(): + rospy.init_node('display', anonymous=True) + + print('Try connect real mycobot...') + port = rospy.get_param('~port', '/dev/ttyUSB0') + baud = rospy.get_param('~baud', 115200) + print('port: {}, baud: {}\n'.format(port, baud)) + try: + mycobot = MyCobot(port, baud) + except Exception as e: + print(e) + print('''\ + \rTry connect mycobot failed! + \rPlease check wether connected with mycobot. + \rPlease chckt wether the port or baud is right. + ''') + exit(1) + mycobot.release_all_servos() + time.sleep(.1) + print('Rlease all servos over.\n') + pub = rospy.Publisher('joint_states', JointState, queue_size=10) pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10) - rospy.init_node('display', anonymous=True) rate = rospy.Rate(30) # 30hz # pub joint state @@ -36,6 +54,7 @@ def talker(): marker_.header.frame_id = '/joint1' marker_.ns = 'my_namespace' + print('publishing ...') while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() @@ -44,7 +63,7 @@ def talker(): for index, value in enumerate(angles): data_list.append(value) - rospy.loginfo('{}'.format(data_list)) + # rospy.loginfo('{}'.format(data_list)) joint_state_send.position = data_list pub.publish(joint_state_send) @@ -77,9 +96,6 @@ def talker(): if __name__ == '__main__': - port = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True).decode() - mycobot = MyCobot(port) try: talker() except rospy.ROSInterruptException: diff --git a/scripts/listen_real.py b/scripts/listen_real.py index b9085c6..8549633 100755 --- a/scripts/listen_real.py +++ b/scripts/listen_real.py @@ -6,7 +6,7 @@ import math import rospy from sensor_msgs.msg import JointState from std_msgs.msg import Header -from myCobotROS.srv import GetAngles +from mycobot_ros.srv import GetAngles def talker(): diff --git a/scripts/mycobot_ros.py b/scripts/mycobot_services.py similarity index 97% rename from scripts/mycobot_ros.py rename to scripts/mycobot_services.py index 9ed34a8..b6f2940 100755 --- a/scripts/mycobot_ros.py +++ b/scripts/mycobot_services.py @@ -1,7 +1,7 @@ #!/usr/bin/env python2 import time import rospy -from myCobotROS.srv import * +from mycobot_ros.srv import * from pymycobot.mycobot import MyCobot @@ -74,10 +74,8 @@ def get_coords(req): def switch_status(req): if mc: if req.Status: - print(1) mc.set_gripper_state(0, 80) else: - print(2) mc.set_gripper_state(1, 80) diff --git a/scripts/control_slider.py b/scripts/slider_control.py similarity index 77% rename from scripts/control_slider.py rename to scripts/slider_control.py index f565676..3da56fa 100755 --- a/scripts/control_slider.py +++ b/scripts/slider_control.py @@ -9,11 +9,13 @@ from sensor_msgs.msg import JointState from pymycobot.mycobot import MyCobot +mc = None + + def callback(data): #rospy.loginfo(rospy.get_caller_id() + "%s", data.position) # print(data.position) data_list = [] - print(data.position) for index, value in enumerate(data.position): data_list.append(value) @@ -22,19 +24,19 @@ def callback(data): def listener(): + global mc rospy.init_node('control_slider', anonymous=True) rospy.Subscriber("joint_states", JointState, callback) + port = rospy.get_param('~port', '/dev/ttyUSB0') + baud = rospy.get_param('~baud', 115200) + print(port, baud) + mc = MyCobot(port, baud) # spin() simply keeps python from exiting until this node is stopped + print('sping ...') rospy.spin() if __name__ == '__main__': - print('sart') - port = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True).decode() - print(port) - mc = MyCobot(port) - print(mc) listener() diff --git a/scripts/sync_signal.py b/scripts/sync_signal.py new file mode 100755 index 0000000..74f8b7f --- /dev/null +++ b/scripts/sync_signal.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python2 +import time, subprocess +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +def callback(data): + rospy.loginfo(rospy.get_caller_id() + "%s", data) + data_list = [] + for index, value in enumerate(data.position): + # if index != 2: + # value *= -1 + data_list.append(value) + + mc.send_radians(data_list, 80) + +def listener(): + rospy.init_node('mycobot_reciver', anonymous=True) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == '__main__': + port = subprocess.check_output(['echo -n /dev/ttyUSB*'], + shell=True) + mc = MyCobot(port) + listener() \ No newline at end of file diff --git a/scripts/teleop_keyboard.py b/scripts/teleop_keyboard.py index 7e520ff..c52b336 100755 --- a/scripts/teleop_keyboard.py +++ b/scripts/teleop_keyboard.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from __future__ import print_function -from myCobotROS.srv import ( +from mycobot_ros.srv import ( GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) import rospy import sys @@ -11,17 +11,19 @@ import copy import time import roslib -roslib.load_manifest('myCobotROS') +roslib.load_manifest('mycobot_ros') msg = """ Mycobot Teleop Keyboard Controller --------------------------- -Movimg options(control coord [x,y,z,rx,ry,rz]): +Movimg options(control coordinations [x,y,z,rx,ry,rz]): w(x+) a(y-) s(x-) d(y+) + z(z-) x(z+) + u(rx+) i(ry+) o(rz+) j(rx-) k(ry-) l(rz-) @@ -31,27 +33,29 @@ Gripper control: Other: 1 - Go to init pose - 2 - - 3 - + 2 - Go to home pose + 3 - Resave home pose q - Quit """ -def getKey(key_timeout): - tty.setraw(sys.stdin.fileno()) - rlist, _, _ = select.select([sys.stdin], [], [], key_timeout) - if rlist: - key = sys.stdin.read(1) - else: - key = '' - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key - - def vels(speed, turn): return "currently:\tspeed %s\tchange size %s " % (speed, turn) +class Raw(object): + def __init__(self,stream): + self.stream = stream + self.fd = self.stream.fileno() + + def __enter__(self): + self.original_stty = termios.tcgetattr(self.stream) + tty.setcbreak(self.stream) + + def __exit__(self, type, value, traceback): + termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty) + + def teleop_keyboard(): global settings settings = termios.tcgetattr(sys.stdin) @@ -88,7 +92,7 @@ def teleop_keyboard(): while True: res = get_coords() - if res.x > 5: + if res.x > 1: break time.sleep(.1) @@ -102,7 +106,9 @@ def teleop_keyboard(): print(vels(speed, change_size)) while(1): try: - key = getKey(key_timeout) + print("\r current coords: %s" % record_coords, end="") + with Raw(sys.stdin): + key = sys.stdin.read(1) if key == 'q': break elif key in ['w', 'W']: @@ -158,14 +164,13 @@ def teleop_keyboard(): home_pose[5] = rep.joint_5 else: continue + except Exception as e: # print(e) continue except Exception as e: print(e) - finally: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) if __name__ == "__main__": diff --git a/src/camera_display.cpp b/src/camera_display.cpp new file mode 100644 index 0000000..8df928d --- /dev/null +++ b/src/camera_display.cpp @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +void imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); + cv::waitKey(30); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + cv::startWindowThread(); + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe("camera/image", 1,imageCallback); + ros::spin(); + cv::destroyWindow("view"); + +} \ No newline at end of file diff --git a/src/opencv_camera.cpp b/src/opencv_camera.cpp new file mode 100644 index 0000000..9ace009 --- /dev/null +++ b/src/opencv_camera.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include // for converting the command line parameter to integer + +int main(int argc, char** argv) +{ + // Check if video source has been passed as a parameter + if(argv[1] == NULL) + { + ROS_INFO("argv[1]=NULL\n"); + return 1; + } + + ros::init(argc, argv, "image_publisher"); // Initialize node + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic + + ros::Rate loop_rate(200); // refresh Hz. + + // Convert the passed as command line parameter index for the video device to an integer + std::istringstream video_sourceCmd(argv[1]); + int video_source; + // Check if it is indeed a number + if(!(video_sourceCmd >> video_source)) + { + ROS_INFO("video_sourceCmd is %d\n",video_source); + return 1; + } + + cv::VideoCapture cap(video_source); + // Check if video device can be opened with the given index + if(!cap.isOpened()) + { + ROS_INFO("can not opencv video device\n"); + return 1; + } + cv::Mat frame; + sensor_msgs::ImagePtr msg; + + while (nh.ok()) + { + cap >> frame; + // cv::imshow("veiwer", frame); + // Check if grabbed frame is actually full with some content + if(!frame.empty()) + { + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + pub.publish(msg); + //cv::Wait(1); + } + ros::spinOnce(); + loop_rate.sleep(); + // if(cv::waitKey(2) >= 0) + // break; + } + +} \ No newline at end of file diff --git a/urdf/mycobot_urdf.urdf b/urdf/mycobot_urdf.urdf index fc403c9..60e05cd 100755 --- a/urdf/mycobot_urdf.urdf +++ b/urdf/mycobot_urdf.urdf @@ -8,7 +8,7 @@ - + @@ -17,7 +17,7 @@ - + @@ -28,7 +28,7 @@ - + @@ -39,7 +39,7 @@ - + @@ -52,7 +52,7 @@ - + @@ -63,7 +63,7 @@ - + @@ -74,7 +74,7 @@ - +