Skip to content

Commit baf1315

Browse files
tonylitianyutonylitianyu
committed
added arm service and bring up camera
1 parent 68dbf2e commit baf1315

File tree

7 files changed

+118
-29
lines changed

7 files changed

+118
-29
lines changed

‎mars_rover/config/mars_rover_control.yaml‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -21,20 +21,20 @@ controller_manager:
2121
type: joint_state_broadcaster/JointStateBroadcaster
2222

2323

24-
# arm_joint_trajectory_controller:
25-
# ros__parameters:
26-
# joints:
27-
# - arm_01_joint
28-
# - arm_02_joint
29-
# - arm_03_joint
30-
# - arm_04_joint
31-
# - arm_tools_joint
32-
# interface_name: position
33-
# command_interfaces:
34-
# - position
35-
# state_interfaces:
36-
# - position
37-
# - velocity
24+
arm_joint_trajectory_controller:
25+
ros__parameters:
26+
joints:
27+
- arm_01_joint
28+
- arm_02_joint
29+
- arm_03_joint
30+
- arm_04_joint
31+
- arm_tools_joint
32+
interface_name: position
33+
command_interfaces:
34+
- position
35+
state_interfaces:
36+
- position
37+
- velocity
3838

3939
# mast_joint_trajectory_controller:
4040
# ros__parameters:

‎mars_rover/launch/mars_rover.launch.py‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -99,11 +99,11 @@ def generate_launch_description():
9999
output='screen'
100100
)
101101

102-
# load_arm_joint_traj_controller = ExecuteProcess(
103-
# cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
104-
# 'arm_joint_trajectory_controller'],
105-
# output='screen'
106-
# )
102+
load_arm_joint_traj_controller = ExecuteProcess(
103+
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
104+
'arm_joint_trajectory_controller'],
105+
output='screen'
106+
)
107107

108108
# load_mast_joint_traj_controller = ExecuteProcess(
109109
# cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
@@ -157,6 +157,7 @@ def generate_launch_description():
157157
OnProcessExit(
158158
target_action=set_hardware_interface_active,
159159
on_exit=[load_joint_state_broadcaster,
160+
load_arm_joint_traj_controller,
160161
load_wheel_joint_traj_controller,
161162
load_steer_joint_traj_controller,
162163
load_suspension_joint_traj_controller],

‎mars_rover/nodes/move_arm‎

Lines changed: 23 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,35 +6,50 @@ from builtin_interfaces.msg import Duration
66

77
from std_msgs.msg import String, Float64
88
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
9+
from std_srvs.srv import Empty
910

1011
class MoveArm(Node):
1112

1213
def __init__(self):
1314
super().__init__('arm_node')
1415
self.arm_publisher_ = self.create_publisher(JointTrajectory, '/arm_joint_trajectory_controller/joint_trajectory', 10)
15-
timer_period = 0.5 # seconds
16-
self.timer = self.create_timer(timer_period, self.timer_callback)
16+
self.pick_srv = self.create_service(Empty, 'move_arm', self.move_arm_callback)
1717

1818

19-
def timer_callback(self):
19+
# timer_period = 20.0 # seconds
20+
# self.timer = self.create_timer(timer_period, self.timer_callback)
21+
22+
def move_arm_callback(self, request, response):
2023
traj = JointTrajectory()
2124
traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
2225

23-
point = JointTrajectoryPoint()
24-
point.positions = [0.0,0.0,0.0,1.0,0.0]
25-
point.time_from_start = Duration(sec=1)
26+
point1 = JointTrajectoryPoint()
27+
point1.positions = [0.0,0.0,1.0,0.0,0.0]
28+
point1.time_from_start = Duration(sec=4)
29+
30+
traj.points.append(point1)
31+
32+
point2 = JointTrajectoryPoint()
33+
point2.positions = [0.0,0.0,0.0,0.0,0.0]
34+
point2.time_from_start = Duration(sec=8)
35+
36+
37+
traj.points.append(point2)
38+
39+
self.get_logger().info(f'Publishing: "{traj.points}"')
2640

27-
traj.points.append(point)
2841
self.arm_publisher_.publish(traj)
2942

43+
return response
44+
45+
3046

3147
def main(args=None):
3248
rclpy.init(args=args)
3349

3450
arm_node = MoveArm()
3551

3652
rclpy.spin(arm_node)
37-
3853
# Destroy the node explicitly
3954
# (optional - otherwise it will be done automatically
4055
# when the garbage collector destroys the node object)

‎mars_rover/nodes/move_wheel‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,6 @@ class MoveWheel(Node):
6868
else:
6969
target_steer.data = [-alpha_o, alpha_o, -alpha_i, alpha_i]
7070

71-
self.get_logger().info(f'Publishing: "{target_steer.data}"')
7271
self.steer_publisher_.publish(target_steer)
7372

7473

‎mars_rover/urdf/curiosity_mars_rover.xacro‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -143,4 +143,42 @@
143143
<parameters>$(find mars_rover)/config/mars_rover_control.yaml</parameters>
144144
</plugin>
145145
</gazebo>
146+
147+
148+
149+
<gazebo reference="camera_link">
150+
<sensor type="camera" name="camera_sensor">
151+
<visualize>1</visualize>
152+
<update_rate>10.0</update_rate>
153+
<camera>
154+
<pose>1.0 0 0 0 0 -1.57</pose>
155+
<horizontal_fov>1.3962634</horizontal_fov>
156+
<image>
157+
<width>800</width>
158+
<height>800</height>
159+
<format>R8G8B8</format>
160+
</image>
161+
<clip>
162+
<near>0.01</near>
163+
<far>100</far>
164+
</clip>
165+
<noise>
166+
<type>gaussian</type>
167+
<stddev>0.007</stddev>
168+
</noise>
169+
</camera>
170+
<always_on>1</always_on>
171+
<topic>image_raw</topic>
172+
<plugin
173+
filename="ignition-gazebo-sensors-system"
174+
name="ignition::gazebo::systems::Sensors">
175+
<render_engine>ogre2</render_engine>
176+
<background_color>1 1 1 1</background_color>
177+
</plugin>
178+
</sensor>
179+
</gazebo>
180+
181+
182+
183+
146184
</robot>

‎mars_rover/urdf/sensor_mast.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -265,7 +265,7 @@
265265
<joint name="head_camera_joint" type="fixed">
266266
<parent link="mast_cameras"/>
267267
<child link="camera_link"/>
268-
<origin xyz="0.13642 -0.218683 0.222314" rpy="0 0 ${PI}"/>
268+
<origin xyz="0.13642 -0.218683 0.222314" rpy="0 0 ${-PI/2}"/>
269269
</joint>
270270

271271
</xacro:macro>

‎mars_rover/worlds/mars_curiosity.world‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,41 @@
11
<sdf version="1.5">
22
<world name="default">
3+
<gui fullscreen="0">
4+
5+
<plugin filename="GzScene3D" name="3D View">
6+
<ignition-gui>
7+
<title>3D View</title>
8+
<property type="bool" key="showTitleBar">false</property>
9+
<property type="string" key="state">docked</property>
10+
</ignition-gui>
11+
<engine>ogre2</engine>
12+
<scene>scene</scene>
13+
<background_color>1.0 1.0 1.0</background_color>
14+
<camera_pose>-5.0 0.0 -6.0 0.0 0.0 0.0</camera_pose>
15+
</plugin>
16+
17+
<plugin filename="ImageDisplay" name="Image Display">
18+
<ignition-gui>
19+
<property key="state" type="string">floating</property>
20+
</ignition-gui>
21+
</plugin>
22+
</gui>
23+
24+
<server_config>
25+
<plugins>
26+
<plugin entity_name="*"
27+
entity_type="world"
28+
filename="ignition-gazebo-scene-broadcaster-system"
29+
name="ignition::gazebo::systems::SceneBroadcaster">
30+
</plugin>
31+
<plugin entity_name="*"
32+
entity_type="world"
33+
filename="ignition-gazebo-sensors-system"
34+
name="ignition::gazebo::systems::Sensors">
35+
<render_engine>ogre</render_engine>
36+
</plugin>
37+
</plugins>
38+
</server_config>
339

440
<!-- Martian Gravity-->
541
<gravity>0 0 -3.711</gravity>

0 commit comments

Comments
 (0)